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

Eigen alignment fix

Browse Source
This commit is contained in:
akira_you
2017-03-25 05:57:53 +09:00
parent 80744bbe42
commit ec240827b1
9 changed files with 36 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/include/svo/depth_filter.h
+1 -1
View File
@@ -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<Seed>& seeds);
void getSeedsCopy(const FramePtr& frame, std::list<Seed, aligned_allocator<Seed> >& seeds);
/// Return a reference to the seeds. This is NOT THREAD SAFE!
std::list<Seed, aligned_allocator<Seed> >& getSeeds() { return seeds_; }
svo/include/svo/global.h
+4
View File
@@ -25,6 +25,10 @@
#include <math.h>
#include <Eigen/Core>
template <class T, class Allocator = Eigen::aligned_allocator<std::pair<const int,T>> >
using vectorA = std::vector< T, Allocator > ;
template <class T, class Allocator = Eigen::aligned_allocator<std::pair<const int,T>> >
using listA = std::list< T, Allocator > ;
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <vikit/performance_monitor.h>
svo/include/svo/initialization.h
+9 -9
View File
@@ -44,11 +44,11 @@ public:
protected:
vector<cv::Point2f> px_ref_; //!< keypoints to be tracked in reference frame.
vector<cv::Point2f> px_cur_; //!< tracked keypoints in current frame.
vector<Vector3d> f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image.
vector<Vector3d> f_cur_; //!< bearing vectors corresponding to the keypoints in the current image.
vectorA<Vector3d> f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image.
vectorA<Vector3d> f_cur_; //!< bearing vectors corresponding to the keypoints in the current image.
vector<double> disparities_; //!< disparity between first and second frame.
vector<int> inliers_; //!< inliers after the geometric check (e.g., Homography).
vector<Vector3d> xyz_in_cur_; //!< 3D points computed during the geometric check.
vectorA<Vector3d> 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<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec);
vectorA<Vector3d>& f_vec);
/// Compute optical flow (Lucas Kanade) for selected keypoints.
void trackKlt(
@@ -64,17 +64,17 @@ void trackKlt(
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vectorA<Vector3d>& f_ref,
vectorA<Vector3d>& f_cur,
vector<double>& disparities);
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
const vectorA<Vector3d>& f_ref,
const vectorA<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
vectorA<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref);
} // namespace initialization
svo/include/svo/reprojector.h
+1 -1
View File
@@ -72,7 +72,7 @@ private:
Vector2d px; //!< projected 2D pixel location.
Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
};
typedef std::list<Candidate, aligned_allocator<Candidate> > Cell;
typedef std::list<Candidate > Cell;
typedef std::vector<Cell*> CandidateGrid;
/// The grid stores a set of candidate matches. For every grid cell we try to find one match.
svo/src/depth_filter.cpp
+4 -4
View File
@@ -135,7 +135,7 @@ void DepthFilter::removeKeyframe(FramePtr frame)
{
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_);
list<Seed>::iterator it=seeds_.begin();
std::list<Seed, aligned_allocator<Seed> >::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<Seed>::iterator it=seeds_.begin();
std::list<Seed, aligned_allocator<Seed> >::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<Seed>& seeds)
void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed, aligned_allocator<Seed> >& seeds)
{
lock_t lock(seeds_mut_);
for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
for(std::list<Seed, aligned_allocator<Seed> >::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
{
if (it->ftr->frame == frame.get())
seeds.push_back(*it);
svo/src/frame_handler_mono.cpp
-1
View File
@@ -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());
}
svo/src/initialization.cpp
+9 -9
View File
@@ -107,7 +107,7 @@ void KltHomographyInit::reset()
void detectFeatures(
FramePtr frame,
vector<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec)
vectorA<Vector3d>& f_vec)
{
Features new_features;
feature_detection::FastDetector detector(
@@ -129,8 +129,8 @@ void trackKlt(
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vectorA<Vector3d>& f_ref,
vectorA<Vector3d>& f_cur,
vector<double>& disparities)
{
const double klt_win_size = 30.0;
@@ -148,7 +148,7 @@ void trackKlt(
vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
vector<Vector3d>::iterator f_ref_it = f_ref.begin();
vectorA<Vector3d>::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<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
const vectorA<Vector3d>& f_ref,
const vectorA<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
vectorA<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
vectorA<Vector2d > uv_ref(f_ref.size());
vectorA<Vector2d > uv_cur(f_cur.size());
for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
{
uv_ref[i] = vk::project2d(f_ref[i]);
svo_ros/src/visualizer.cpp
+8 -9
View File
@@ -134,26 +134,25 @@ void Visualizer::publishMinimal(
cv::Point2f((*it)->px[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<cv::Vec3b>((*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;
svo_ros/src/vo_node.cpp
-1
View File
@@ -13,7 +13,6 @@
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <ros/package.h>
#include <string>
#include <svo/frame_handler_mono.h>