diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 52b8f0f..53fb1bb 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -128,10 +128,10 @@ namespace vio { //TODO add 15 degree roll orientation SE2 inv=T_f_w_.inverse(); Quaterniond q; - q = AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitX()) - * AngleAxisd(0.261799, Vector3d::UnitY()) + q = AngleAxisd(0.261799, Vector3d::UnitX()) + * AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - SE3 out(q.toRotationMatrix(),Vector3d(0.0,inv.translation()(0),inv.translation()(1))); + SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1))); return out; } diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index c78eb5a..f115137 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -92,10 +92,10 @@ namespace vio SE3 se3() const{ //Todo add 15 roll orientation Quaterniond q; - q = AngleAxisd(pitch(), Vector3d::UnitX()) - * AngleAxisd(0.261799, Vector3d::UnitY()) + q = AngleAxisd(0.261799, Vector3d::UnitX()) + * AngleAxisd(pitch(), Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - return SE3(q.toRotationMatrix(),Vector3d(1e-19,T2_.translation()(0),T2_.translation()(1))); + return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0), 0.0,T2_.translation()(1))); } ~SE2_5(){} diff --git a/GPU_version/vio/include/vio/reprojector.h b/GPU_version/vio/include/vio/reprojector.h index b6dd2f3..605265a 100644 --- a/GPU_version/vio/include/vio/reprojector.h +++ b/GPU_version/vio/include/vio/reprojector.h @@ -55,20 +55,24 @@ public: size_t n_matches_; size_t n_trials_; - Reprojector(vk::AbstractCamera* cam, Map& map/*, opencl* kernel*/); + Reprojector(vk::AbstractCamera* cam, Map& map); ~Reprojector(); /// Project points from the map into the image. First finds keyframes with /// overlapping field of view and projects only those map-points. -// void reprojectMap( -// FramePtr frame, -// std::vector< std::pair >& overlap_kfs); + void reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs); - void reprojectMap( - FramePtr frame, - FramePtr framel, - std::vector< std::pair >& overlap_kfs); + void reprojectMap1( + FramePtr frame, + FramePtr framel, + std::vector< std::pair >& overlap_kfs); + + void reprojectMap2( + FramePtr frame, + std::vector< std::pair >& overlap_kfs); private: @@ -102,10 +106,7 @@ private: void resetGrid(); bool reprojectCell(Cell& cell, FramePtr frame); bool reprojectPoint(FramePtr frame, Point* point); - bool reprojectPoint(FramePtr frame, Point* point, float x, float y); - -protected: - opencl* gpu_freak; + bool reprojectPoint1(FramePtr frame, Point* point, float x, float y); }; } // namespace vio diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index 2264590..62fa181 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -218,10 +218,10 @@ namespace vio { SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation()); ///TODO add 15 degrees roll orientation Quaterniond q; - q = AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitX()) - * AngleAxisd(0.261799, Vector3d::UnitY()) + q = AngleAxisd(0.261799, Vector3d::UnitX()) + * AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(0.0,T.translation()(0),T.translation()(1))); + SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1))); const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); if(xyz_f.z() < 0.0) { ++it; // behind the camera diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 5aa77ca..5daa451 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -170,12 +170,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false); SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; -// cv::Mat imgref = last_frame_->img().clone(); -// cv::imwrite("/root/Projects/ROS/src/p_33_vio/GPU_version/imgref.png",imgref); -// cv::Mat imgcur = new_frame_->img().clone(); -// cv::imwrite("/root/Projects/ROS/src/p_33_vio/GPU_version/imgcur.png",imgcur); // reprojector_.reprojectMap(new_frame_, overlap_kfs_); - reprojector_.reprojectMap(new_frame_, last_frame_, overlap_kfs_); + reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); +// reprojector_.reprojectMap2(new_frame_, overlap_kfs_); //assert (false); std::cout<<"Reprojection Map nPoint: "<clear(); }); } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO - Original implementation +void Reprojector::reprojectMap( + FramePtr frame, + std::vector< std::pair >& overlap_kfs) +{ + resetGrid(); -//void Reprojector::reprojectMap( -// FramePtr frame, -// std::vector< std::pair >& overlap_kfs) -//{ -// resetGrid(); -// -// // Identify those Keyframes which share a common field of view. -//// SVO_START_TIMER("reproject_kfs"); -// list< pair > close_kfs; -// map_.getCloseKeyframes(frame, close_kfs); -// -// // Sort KFs with overlap according to their closeness -// close_kfs.sort(boost::bind(&std::pair::second, _1) < -// boost::bind(&std::pair::second, _2)); -// -// // Reproject all mappoints of the closest N kfs with overlap. We only store -// // in which grid cell the points fall. -// size_t n = 0; -// overlap_kfs.reserve(options_.max_n_kfs); -// for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); -// it_frame!=ite_frame && nfirst; -// overlap_kfs.push_back(pair(ref_frame,0)); -// -// // Try to reproject each mappoint that the other KF observes -// for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); -// it_ftr!=ite_ftr; ++it_ftr) -// { -// // check if the feature has a mappoint assigned -// if((*it_ftr)->point == NULL) -// continue; -// -// // make sure we project a point only once -// if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) -// continue; -// (*it_ftr)->point->last_projected_kf_id_ = frame->id_; -// if(reprojectPoint(frame, (*it_ftr)->point)){ -// overlap_kfs.back().second++; -// } -// -// } -// } -// //SVO_STOP_TIMER("reproject_kfs"); -// -// // Now project all point candidates -// // SVO_START_TIMER("reproject_candidates"); -// { -// boost::unique_lock lock(map_.point_candidates_.mut_); -// auto it=map_.point_candidates_.candidates_.begin(); -// while(it!=map_.point_candidates_.candidates_.end()) -// { -// if(!reprojectPoint(frame, it->first)) -// { -// it->first->n_failed_reproj_ += 3; -// if(it->first->n_failed_reproj_ > 30) -// { -// map_.point_candidates_.deleteCandidate(*it); -// it = map_.point_candidates_.candidates_.erase(it); -// continue; -// } -// } -// ++it; -// } -// } // unlock the mutex when out of scope -// //SVO_STOP_TIMER("reproject_candidates"); -// // Now we go through each grid cell and select one point to match. -// // At the end, we should have at maximum one reprojected point per cell. -// //SVO_START_TIMER("feature_align"); -// -//// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//// std::vector kps; // keypoints -//// cl_float4 data; -//// int frame_number = 0; -//// for(auto it_frame = overlap_kfs.begin(), ite_frame = overlap_kfs.end(); it_frame != ite_frame; it_frame++){ -//// for(auto it_ftr = it_frame->first->fts_.begin(), ite_ftr = it_frame->first->fts_.end(); it_ftr != ite_ftr; it_ftr++){ -//// data.w = frame_number; -//// data.x = (*it_ftr)->px.x(); -//// data.y = (*it_ftr)->px.y(); -//// data.z = 0.0; -//// kps.push_back(data); -//// } -//// frame_number++; -//// } -// -// -//// cv::Mat imgbegin = overlap_kfs.begin()->first->img().clone(); -//// cv::imwrite("/root/Projects/ROS/src/p_33_vio/GPU_version/img_overlap_kfs_begin.png",imgbegin); -//// cv::Mat imgbend = overlap_kfs.end()->first->img().clone(); -//// cv::imwrite("/root/Projects/ROS/src/p_33_vio/GPU_version/img_overlap_kfs_end.png",imgbend); -// -//// ofstream fileref; -//// fileref.open("/root/Projects/ROS/src/p_33_vio/GPU_version/dataref.txt"); -//// //cl_float4 keypoints[kps.size()]; -//// //size_t counter = 0; -//// for(auto it = kps.begin(), ite = kps.end(); it != ite; it++) { -//// cl_float4 ftr; -//// ftr.w = it->w; -//// ftr.x = it->x; -//// ftr.y = it->y; -//// ftr.z = it->z; -//// //keypoints[counter++] = ftr; -//// fileref<begin(), ite = cell->end(); it != ite; it++, counter++) -//// { -//// //keypoints[counter].x = it->px.x(); //x -//// //keypoints[counter].y = it->px.y(); //y -//// //keypoints[counter].z = 0.0; //angle -//// filecur<px.x()<<","<px.y()<<","<<0.0<img(); -//// cv::Mat integral_img=cv::Mat(cv::Size(701, 501),CV_8UC1); -//// gpu_freak->reload_buf(0,0,img); -//// gpu_freak->run(0); -//// gpu_freak->read(0,1,1,&integral_img); -// -//// // FREAK preprocessing -//// static const float FREAK_LOG2 = 0.693147180559945; -//// static const int FREAK_NB_ORIENTATION = 256; -//// static const int NB_ORIENPAIRS = 45; -//// static const int FREAK_NB_POINTS = 43; -//// static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints -//// static const int NB_PAIRS = 512; -//// static const int NB_SCALES = 64; -//// -//// bool orientationNormalized = true; -//// bool scaleNormalized = true; -//// float patternScale = 22.0f; -//// int nOctaves = 4; -//// -//// bool extAll = false; // true if all pairs need to be extracted for pairs selection -//// -//// cl_float3 patternLookup[NB_SCALES * FREAK_NB_ORIENTATION * FREAK_NB_POINTS]; // look-up table for the pattern points (position+sigma of all points at all scales and orientation) -//// int patternSizes[NB_SCALES]; // size of the pattern at a specific scale (used to check if a point is within image boundaries) -//// cl_uchar2 descriptionPairs[NB_PAIRS]; -//// cl_int4 orientationPairs[NB_ORIENPAIRS]; -//// -//// // default pairs -//// static const int FREAK_DEF_PAIRS[NB_PAIRS] = { -//// 404,431,818,511,181,52,311,874,774,543,719,230,417,205,11, -//// 560,149,265,39,306,165,857,250,8,61,15,55,717,44,412, -//// 592,134,761,695,660,782,625,487,549,516,271,665,762,392,178, -//// 796,773,31,672,845,548,794,677,654,241,831,225,238,849,83, -//// 691,484,826,707,122,517,583,731,328,339,571,475,394,472,580, -//// 381,137,93,380,327,619,729,808,218,213,459,141,806,341,95, -//// 382,568,124,750,193,749,706,843,79,199,317,329,768,198,100, -//// 466,613,78,562,783,689,136,838,94,142,164,679,219,419,366, -//// 418,423,77,89,523,259,683,312,555,20,470,684,123,458,453,833, -//// 72,113,253,108,313,25,153,648,411,607,618,128,305,232,301,84, -//// 56,264,371,46,407,360,38,99,176,710,114,578,66,372,653, -//// 129,359,424,159,821,10,323,393,5,340,891,9,790,47,0,175,346, -//// 236,26,172,147,574,561,32,294,429,724,755,398,787,288,299, -//// 769,565,767,722,757,224,465,723,498,467,235,127,802,446,233, -//// 544,482,800,318,16,532,801,441,554,173,60,530,713,469,30, -//// 212,630,899,170,266,799,88,49,512,399,23,500,107,524,90, -//// 194,143,135,192,206,345,148,71,119,101,563,870,158,254,214, -//// 276,464,332,725,188,385,24,476,40,231,620,171,258,67,109, -//// 844,244,187,388,701,690,50,7,850,479,48,522,22,154,12,659, -//// 736,655,577,737,830,811,174,21,237,335,353,234,53,270,62, -//// 182,45,177,245,812,673,355,556,612,166,204,54,248,365,226, -//// 242,452,700,685,573,14,842,481,468,781,564,416,179,405,35, -//// 819,608,624,367,98,643,448,2,460,676,440,240,130,146,184, -//// 185,430,65,807,377,82,121,708,239,310,138,596,730,575,477, -//// 851,797,247,27,85,586,307,779,326,494,856,324,827,96,748, -//// 13,397,125,688,702,92,293,716,277,140,112,4,80,855,839,1, -//// 413,347,584,493,289,696,19,751,379,76,73,115,6,590,183,734, -//// 197,483,217,344,330,400,186,243,587,220,780,200,793,246,824, -//// 41,735,579,81,703,322,760,720,139,480,490,91,814,813,163, -//// 152,488,763,263,425,410,576,120,319,668,150,160,302,491,515, -//// 260,145,428,97,251,395,272,252,18,106,358,854,485,144,550, -//// 131,133,378,68,102,104,58,361,275,209,697,582,338,742,589, -//// 325,408,229,28,304,191,189,110,126,486,211,547,533,70,215, -//// 670,249,36,581,389,605,331,518,442,822 -//// }; -//// -//// float scaleStep = pow(2.0, (float)(nOctaves)/NB_SCALES ); // 2 ^ ( (nOctaves-1) /nbScales) -//// -//// const int nn[8] = {6, 6, 6, 6, 6, 6, 6, 1}; // number of points on each concentric circle (from outer to inner) -//// const float bigR = (float)(2.0 / 3.0); // bigger radius -//// const float smallR = (float)(2.0 / 24.0); // smaller radius -//// const float unitSpace = (float)((bigR - smallR) / 21.0 ); // define spaces between concentric circles (from center to outer: 1,2,3,4,5,6) -//// // radii of the concentric cirles (from outer to inner) -//// const float radius[8] = {bigR , bigR - 6 * unitSpace, bigR - 11 * unitSpace, bigR - 15 * unitSpace, -//// bigR - 18 * unitSpace, bigR - 20 * unitSpace, smallR , 0.0}; -//// // sigma of pattern points (each group of 6 points on a concentric cirle has the same sigma) -//// const float sigma[8] = {float(radius[0] / 2.0), float(radius[1] / 2.0), float(radius[2] / 2.0), float(radius[3] / 2.0), -//// float(radius[4] / 2.0), float(radius[5] / 2.0), float(radius[6] / 2.0), float(radius[6] / 2.0)}; -//// -//// for( int scaleIdx=0; scaleIdx < NB_SCALES; ++scaleIdx ) -//// { -//// patternSizes[scaleIdx] = 0; // proper initialization -//// float scalingFactor = pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx -//// -//// for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx ) -//// { -//// int pointIdx = 0; -//// -//// for( size_t i = 0; i < 8; ++i ) -//// { -//// for( int k = 0 ; k < nn[i]; ++k ) -//// { -//// float alpha = (float)(k) * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(nn[i]) -//// + ((float)3.141592653589793238462643383279502884197169399375 / nn[i] * ( i % 2 )/*beta*/) -//// + ((float)(orientationIdx * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(FREAK_NB_ORIENTATION)) /* theta*/); -//// -//// // add the point to the look-up table -//// cl_float3* point = &patternLookup[scaleIdx * FREAK_NB_ORIENTATION * FREAK_NB_POINTS + orientationIdx * FREAK_NB_POINTS + pointIdx]; -//// point->x = (float)(radius[i] * cos(alpha) * scalingFactor * patternScale); //x -//// point->y = (float)(radius[i] * sin(alpha) * scalingFactor * patternScale); //y -//// point->z = (float)(sigma[i] * scalingFactor * patternScale); //sigma -//// -//// // adapt the sizeList if necessary -//// const int sizeMax = (int)(ceil((radius[i] + sigma[i]) * scalingFactor * patternScale)) + 1; -//// if( patternSizes[scaleIdx] < sizeMax ) -//// patternSizes[scaleIdx] = sizeMax; -//// -//// ++pointIdx; -//// } -//// } -//// } -//// } -//// -//// // build the list of orientation pairs -//// orientationPairs[0].w=0; orientationPairs[0].x=3; orientationPairs[1].w=1; orientationPairs[1].x=4; orientationPairs[2].w=2; orientationPairs[2].x=5; -//// orientationPairs[3].w=0; orientationPairs[3].x=2; orientationPairs[4].w=1; orientationPairs[4].x=3; orientationPairs[5].w=2; orientationPairs[5].x=4; -//// orientationPairs[6].w=3; orientationPairs[6].x=5; orientationPairs[7].w=4; orientationPairs[7].x=0; orientationPairs[8].w=5; orientationPairs[8].x=1; -//// -//// orientationPairs[9].w=6; orientationPairs[9].x=9; orientationPairs[10].w=7; orientationPairs[10].x=10; orientationPairs[11].w=8; orientationPairs[11].x=11; -//// orientationPairs[12].w=6; orientationPairs[12].x=8; orientationPairs[13].w=7; orientationPairs[13].x=9; orientationPairs[14].w=8; orientationPairs[14].x=10; -//// orientationPairs[15].w=9; orientationPairs[15].x=11; orientationPairs[16].w=10; orientationPairs[16].x=6; orientationPairs[17].w=11; orientationPairs[17].x=7; -//// -//// orientationPairs[18].w=12; orientationPairs[18].x=15; orientationPairs[19].w=13; orientationPairs[19].x=16; orientationPairs[20].w=14; orientationPairs[20].x=17; -//// orientationPairs[21].w=12; orientationPairs[21].x=14; orientationPairs[22].w=13; orientationPairs[22].x=15; orientationPairs[23].w=14; orientationPairs[23].x=16; -//// orientationPairs[24].w=15; orientationPairs[24].x=17; orientationPairs[25].w=16; orientationPairs[25].x=12; orientationPairs[26].w=17; orientationPairs[26].x=13; -//// -//// orientationPairs[27].w=18; orientationPairs[27].x=21; orientationPairs[28].w=19; orientationPairs[28].x=22; orientationPairs[29].w=20; orientationPairs[29].x=23; -//// orientationPairs[30].w=18; orientationPairs[30].x=20; orientationPairs[31].w=19; orientationPairs[31].x=21; orientationPairs[32].w=20; orientationPairs[32].x=22; -//// orientationPairs[33].w=21; orientationPairs[33].x=23; orientationPairs[34].w=22; orientationPairs[34].x=18; orientationPairs[35].w=23; orientationPairs[35].x=19; -//// -//// orientationPairs[36].w=24; orientationPairs[36].x=27; orientationPairs[37].w=25; orientationPairs[37].x=28; orientationPairs[38].w=26; orientationPairs[38].x=29; -//// orientationPairs[39].w=30; orientationPairs[39].x=33; orientationPairs[40].w=31; orientationPairs[40].x=34; orientationPairs[41].w=32; orientationPairs[41].x=35; -//// orientationPairs[42].w=36; orientationPairs[42].x=39; orientationPairs[43].w=37; orientationPairs[43].x=40; orientationPairs[44].w=38; orientationPairs[44].x=41; -//// -//// for( unsigned m = NB_ORIENPAIRS; m--; ) -//// { -//// float dx = patternLookup[orientationPairs[m].w].x - patternLookup[orientationPairs[m].x].x; -//// float dy = patternLookup[orientationPairs[m].w].y - patternLookup[orientationPairs[m].x].y; -//// float norm_sq = (dx * dx + dy * dy); -//// orientationPairs[m].y = round((dx / (norm_sq)) * 4096.0); //weight_dx -//// orientationPairs[m].z = round((dy / (norm_sq)) * 4096.0); //weight_dy -//// } -//// -//// // build the list of description pairs -//// cl_uchar2 allPairs[(int)((FREAK_NB_POINTS * (FREAK_NB_POINTS + 1)) / 2)]; -//// for( int i = 1, idx = 0; i < FREAK_NB_POINTS; ++i ) -//// { -//// // (generate all the pairs) -//// for( int j = 0; j < i; ++j ) -//// { -//// allPairs[idx++] = (cl_uchar2){(uchar)i, (uchar)j}; -//// } -//// } -//// for( int i = 0; i < NB_PAIRS; ++i ) -//// descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]]; -//// -//// int kpScaleIdx[overlap_kfs.back().second]; -// -// -//// gpu_freak->reload_buf(1,0,img); -//// gpu_freak->reload_buf(1,1,integral_img); -//// gpu_freak->reload_buf(1,2, keypoints); -//// gpu_freak->reload_buf(1,4,patternSizes); -//// gpu_freak->reload_buf(1,5,orientationPairs); -//// gpu_freak->reload_buf(1,6,patternLookup); -//// gpu_freak->reload_buf(1,7,descriptionPairs); -//// gpu_freak->write_buf(1,8,nOctaves); -//// gpu_freak->reload_buf(1,9,kpScaleIdx); -//// gpu_freak->run(1); -//// cl_uchar descriptor[frame_number*300*64]={0}; -//// gpu_freak->read(1,3,frame_number*300*64,descriptor); -// -// -// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// -// for(size_t i=0; i (size_t) Config::maxFts()) -// break; -// } -//// SVO_STOP_TIMER("feature_align"); -//} + // Identify those Keyframes which share a common field of view. + // SVO_START_TIMER("reproject_kfs"); + list< pair > close_kfs; + map_.getCloseKeyframes(frame, close_kfs); - void Reprojector::reprojectMap( + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + // Reproject all mappoints of the closest N kfs with overlap. We only store + // in which grid cell the points fall. + size_t n = 0; + overlap_kfs.reserve(options_.max_n_kfs); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + overlap_kfs.push_back(pair(ref_frame,0)); + + // Try to reproject each mappoint that the other KF observes + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + // check if the feature has a mappoint assigned + if((*it_ftr)->point == NULL) + continue; + + // make sure we project a point only once + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + if(reprojectPoint(frame, (*it_ftr)->point)){ + overlap_kfs.back().second++; + } + } + } + //SVO_STOP_TIMER("reproject_kfs"); + + // Now project all point candidates + // SVO_START_TIMER("reproject_candidates"); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + auto it=map_.point_candidates_.candidates_.begin(); + while(it!=map_.point_candidates_.candidates_.end()) + { + if(!reprojectPoint(frame, it->first)) + { + it->first->n_failed_reproj_ += 3; + if(it->first->n_failed_reproj_ > 30) + { + map_.point_candidates_.deleteCandidate(*it); + it = map_.point_candidates_.candidates_.erase(it); + continue; + } + } + ++it; + } + } // unlock the mutex when out of scope + //SVO_STOP_TIMER("reproject_candidates"); + // Now we go through each grid cell and select one point to match. + // At the end, we should have at maximum one reprojected point per cell. + //SVO_START_TIMER("feature_align"); + for(size_t i=0; i (size_t) Config::maxFts()) + break; + } +// SVO_STOP_TIMER("feature_align"); +} +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO - First reimplementation + void Reprojector::reprojectMap1( FramePtr frame, FramePtr framel, std::vector< std::pair >& overlap_kfs) @@ -416,7 +185,7 @@ void Reprojector::resetGrid() auto fts2 = fts; for(int f = 0; f < matches[i].queryIdx; f++) fts2++; - if(reprojectPoint(frame, (*fts2)->point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)){ + if(reprojectPoint1(frame, (*fts2)->point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)){ overlap_kfs.back().second++; } keypoints_cur[i].class_id = 0; @@ -426,7 +195,7 @@ void Reprojector::resetGrid() { if(keypoints_cur[i].class_id == 0) continue; - if(reprojectPoint(frame, nullptr, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) + if(reprojectPoint1(frame, nullptr, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) overlap_kfs.back().second++; } //SVO_STOP_TIMER("reproject_kfs"); @@ -466,7 +235,124 @@ void Reprojector::resetGrid() } // SVO_STOP_TIMER("feature_align"); } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO - second reimplementation + void Reprojector::reprojectMap2( + FramePtr frame, + std::vector< std::pair >& overlap_kfs) + { + resetGrid(); + list< pair > close_kfs; + std::vector keypoints_kfs; + std::vector> keypoints_ref; + map_.getCloseKeyframes(frame, close_kfs); + std::cerr<<"close_kfs size : "<::second, _1) < + boost::bind(&std::pair::second, _2)); + + // Reproject all mappoints of the closest N kfs with overlap. We only store + // in which grid cell the points fall. + size_t n = 0; + overlap_kfs.reserve(options_.max_n_kfs); + keypoints_ref.clear(); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + + // Try to reproject each mappoint that the other KF observes + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + // check if the feature has a mappoint assigned + if((*it_ftr)->point == NULL) + continue; + + // make sure we project a point only once + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + cv::KeyPoint * kp = new cv::KeyPoint; + kp->pt.x = (*it_ftr)->px.x(); + kp->pt.y = (*it_ftr)->px.y(); + keypoints_kfs.push_back(*kp); + } + std::cerr<<"keypoints_kfs size : "< keypoints_cur; + cv::FAST(frame->img(),keypoints_cur,50); + std::vector matches; + cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4); + cv::Ptr matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true); + int ii = 0; + extractor->compute(frame->img(), keypoints_cur, descriptors_cur); + for(auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end(); + it_frame != ite_frame; ++it_frame, ++ii) + { + extractor->compute(it_frame->first->img(), keypoints_ref[ii], descriptors_ref); + matcher->match(descriptors_ref, descriptors_cur, matches); + //std::cerr<<"matches size : "<first; + overlap_kfs.push_back(pair(ref_frame,0)); + + for (int i; i < matches.size(); i++) { + auto fts = ref_frame->fts_.begin(); + auto fts2 = fts; + for (int f = 0; f < matches[i].queryIdx; f++) + fts2++; + if (reprojectPoint1(frame, (*fts2)->point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) { + std::cerr<<"i : "< lock(map_.point_candidates_.mut_); + auto it=map_.point_candidates_.candidates_.begin(); + while(it!=map_.point_candidates_.candidates_.end()) + { + if(!reprojectPoint(frame, it->first)) + { + it->first->n_failed_reproj_ += 3; + if(it->first->n_failed_reproj_ > 30) + { + map_.point_candidates_.deleteCandidate(*it); + it = map_.point_candidates_.candidates_.erase(it); + continue; + } + } + ++it; + } + + for(size_t i=0; i (size_t) Config::maxFts()) + break; + } + } // unlock the mutex when out of scope + } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) { if(lhs.pt->type_ > rhs.pt->type_) @@ -527,7 +413,8 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) } return false; } - +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO original implementation bool Reprojector::reprojectPoint(FramePtr frame, Point* point) { Vector2d px(frame->w2c(point->pos_)); @@ -540,8 +427,9 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point) } return false; } - - bool Reprojector::reprojectPoint(FramePtr frame, Point* point, float x, float y) +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO first reimplementation + bool Reprojector::reprojectPoint1(FramePtr frame, Point* point, float x, float y) { if(point) { Vector2d px(x, y); @@ -571,3 +459,4 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point) } } // namespace vio +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// \ No newline at end of file