diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h index 7c4ab19..6333e54 100644 --- a/svo/include/svo/depth_filter.h +++ b/svo/include/svo/depth_filter.h @@ -116,7 +116,7 @@ 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& seeds); + void getSeedsCopy(const FramePtr& frame, std::list >& seeds); /// Return a reference to the seeds. This is NOT THREAD SAFE! std::list >& getSeeds() { return seeds_; } diff --git a/svo/include/svo/global.h b/svo/include/svo/global.h index 9f7cc97..7a3989b 100644 --- a/svo/include/svo/global.h +++ b/svo/include/svo/global.h @@ -25,6 +25,10 @@ #include #include +template > > +using vectorA = std::vector< T, Allocator > ; +template > > +using listA = std::list< T, Allocator > ; #include #include #include diff --git a/svo/include/svo/initialization.h b/svo/include/svo/initialization.h index cb562e5..ec74ccd 100644 --- a/svo/include/svo/initialization.h +++ b/svo/include/svo/initialization.h @@ -44,11 +44,11 @@ public: protected: vector px_ref_; //!< keypoints to be tracked in reference frame. vector px_cur_; //!< tracked keypoints in current frame. - vector f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. - vector f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. + vectorA f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. + vectorA f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. vector disparities_; //!< disparity between first and second frame. vector inliers_; //!< inliers after the geometric check (e.g., Homography). - vector xyz_in_cur_; //!< 3D points computed during the geometric check. + vectorA xyz_in_cur_; //!< 3D points computed during the geometric check. SE3 T_cur_from_ref_; //!< computed transformation between the first two frames. }; @@ -56,7 +56,7 @@ protected: void detectFeatures( FramePtr frame, vector& px_vec, - vector& f_vec); + vectorA& f_vec); /// Compute optical flow (Lucas Kanade) for selected keypoints. void trackKlt( @@ -64,17 +64,17 @@ void trackKlt( FramePtr frame_cur, vector& px_ref, vector& px_cur, - vector& f_ref, - vector& f_cur, + vectorA& f_ref, + vectorA& f_cur, vector& disparities); void computeHomography( - const vector& f_ref, - const vector& f_cur, + const vectorA& f_ref, + const vectorA& f_cur, double focal_length, double reprojection_threshold, vector& inliers, - vector& xyz_in_cur, + vectorA& xyz_in_cur, SE3& T_cur_from_ref); } // namespace initialization diff --git a/svo/include/svo/reprojector.h b/svo/include/svo/reprojector.h index 7207b54..e345f0d 100644 --- a/svo/include/svo/reprojector.h +++ b/svo/include/svo/reprojector.h @@ -72,7 +72,7 @@ private: Vector2d px; //!< projected 2D pixel location. Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} }; - typedef std::list > Cell; + typedef std::list Cell; typedef std::vector CandidateGrid; /// The grid stores a set of candidate matches. For every grid cell we try to find one match. diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp index bf3a686..4eb6249 100644 --- a/svo/src/depth_filter.cpp +++ b/svo/src/depth_filter.cpp @@ -135,7 +135,7 @@ void DepthFilter::removeKeyframe(FramePtr frame) { seeds_updating_halt_ = true; lock_t lock(seeds_mut_); - list::iterator it=seeds_.begin(); + std::list >::iterator it=seeds_.begin(); size_t n_removed = 0; while(it!=seeds_.end()) { @@ -200,7 +200,7 @@ void DepthFilter::updateSeeds(FramePtr frame) // for all the seeds in every frame! size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size(); lock_t lock(seeds_mut_); - list::iterator it=seeds_.begin(); + std::list >::iterator it=seeds_.begin(); const double focal_length = frame->cam_->errorMultiplier2(); double px_noise = 1.0; @@ -296,10 +296,10 @@ void DepthFilter::clearFrameQueue() frame_queue_.pop(); } -void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list& seeds) +void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list >& seeds) { lock_t lock(seeds_mut_); - for(std::list::iterator it=seeds_.begin(); it!=seeds_.end(); ++it) + for(std::list >::iterator it=seeds_.begin(); it!=seeds_.end(); ++it) { if (it->ftr->frame == frame.get()) seeds.push_back(*it); diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 3050ba7..4facc51 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/svo/src/frame_handler_mono.cpp @@ -84,7 +84,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp) // set last frame last_frame_ = new_frame_; new_frame_.reset(); - // finish processing finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); } diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp index 84b9baf..d8f947a 100644 --- a/svo/src/initialization.cpp +++ b/svo/src/initialization.cpp @@ -107,7 +107,7 @@ void KltHomographyInit::reset() void detectFeatures( FramePtr frame, vector& px_vec, - vector& f_vec) + vectorA& f_vec) { Features new_features; feature_detection::FastDetector detector( @@ -129,8 +129,8 @@ void trackKlt( FramePtr frame_cur, vector& px_ref, vector& px_cur, - vector& f_ref, - vector& f_cur, + vectorA& f_ref, + vectorA& f_cur, vector& disparities) { const double klt_win_size = 30.0; @@ -148,7 +148,7 @@ void trackKlt( vector::iterator px_ref_it = px_ref.begin(); vector::iterator px_cur_it = px_cur.begin(); - vector::iterator f_ref_it = f_ref.begin(); + vectorA::iterator f_ref_it = f_ref.begin(); f_cur.clear(); f_cur.reserve(px_cur.size()); disparities.clear(); disparities.reserve(px_cur.size()); for(size_t i=0; px_ref_it != px_ref.end(); ++i) @@ -169,16 +169,16 @@ void trackKlt( } void computeHomography( - const vector& f_ref, - const vector& f_cur, + const vectorA& f_ref, + const vectorA& f_cur, double focal_length, double reprojection_threshold, vector& inliers, - vector& xyz_in_cur, + vectorA& xyz_in_cur, SE3& T_cur_from_ref) { - vector > uv_ref(f_ref.size()); - vector > uv_cur(f_cur.size()); + vectorA uv_ref(f_ref.size()); + vectorA uv_cur(f_cur.size()); for(size_t i=0, i_max=f_ref.size(); ipx[0]+3*(*it)->grad[1], (*it)->px[1]-3*(*it)->grad[0]), cv::Point2f((*it)->px[0]-3*(*it)->grad[1], (*it)->px[1]+3*(*it)->grad[0]), cv::Scalar(255,0,255), 2); - else + else//point size 5x5 cv::rectangle(img_rgb, cv::Point2f((*it)->px[0]-2, (*it)->px[1]-2), cv::Point2f((*it)->px[0]+2, (*it)->px[1]+2), cv::Scalar(0,255,0), CV_FILLED); } } - if(img_pub_level_ == 1) + else if(img_pub_level_ == 1){//point size 3x3 for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) cv::rectangle(img_rgb, cv::Point2f((*it)->px[0]/scale-1, (*it)->px[1]/scale-1), cv::Point2f((*it)->px[0]/scale+1, (*it)->px[1]/scale+1), cv::Scalar(0,255,0), CV_FILLED); - else - for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) - cv::rectangle(img_rgb, - cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale), - cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale), - cv::Scalar(0,255,0), CV_FILLED); - + }else{ //point size 1x1 + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it){ + cv::Vec3b &p= img_rgb.at((*it)->px[1]/scale, (*it)->px[0]/scale); + p[0]=0;p[1]=255;p[2]=0; + } + } cv_bridge::CvImage img_msg; img_msg.header = header_msg; img_msg.image = img_rgb; diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp index 8a6f88c..c9fbb84 100644 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -13,7 +13,6 @@ // // You should have received a copy of the GNU General Public License // along with this program. If not, see . - #include #include #include