all changes turned to comment
This commit is contained in:
VahdPourAmin
File diff suppressed because it is too large
Load Diff
@@ -3626,10 +3626,13 @@ CMakeFiles/vio.dir/src/pose_optimizer.cpp.o
|
||||
/usr/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o
|
||||
../include/vio/abstract_camera.h
|
||||
../include/vio/cl.hpp
|
||||
../include/vio/cl_class.h
|
||||
../include/vio/config.h
|
||||
../include/vio/feature.h
|
||||
../include/vio/frame.h
|
||||
../include/vio/global.h
|
||||
../include/vio/initialization.h
|
||||
../include/vio/map.h
|
||||
../include/vio/matcher.h
|
||||
../include/vio/math_utils.h
|
||||
|
||||
@@ -3625,10 +3625,13 @@ CMakeFiles/vio.dir/src/pose_optimizer.cpp.o: /usr/include/eigen3/Eigen/src/plugi
|
||||
CMakeFiles/vio.dir/src/pose_optimizer.cpp.o: /usr/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.h
|
||||
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/abstract_camera.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/cl.hpp
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/cl_class.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/config.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/feature.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/frame.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/global.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/initialization.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/map.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/matcher.h
|
||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/math_utils.h
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -55,7 +55,7 @@ public:
|
||||
size_t n_matches_;
|
||||
size_t n_trials_;
|
||||
|
||||
Reprojector(vk::AbstractCamera* cam, Map& map, opencl* kernel);
|
||||
Reprojector(vk::AbstractCamera* cam, Map& map/*, opencl* kernel*/);
|
||||
|
||||
~Reprojector();
|
||||
|
||||
|
||||
@@ -8,7 +8,7 @@ float2 world2cam(float3 feature)
|
||||
{
|
||||
float r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
|
||||
float factor = 1.0;
|
||||
if((float)S != 0 || r > 0.001)
|
||||
if((float)S != 0 && r > 0.001)
|
||||
factor = (atan(r * 2.0 * tan(0.5 * (float)S)) / (r * (float)S));
|
||||
return (float2)((float)C_X + (float)F_X * factor * feature.x/feature.z, (float)C_Y + (float)F_Y * factor * feature.y/feature.z);
|
||||
}
|
||||
@@ -22,8 +22,18 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
|
||||
float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
|
||||
float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
|
||||
return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
|
||||
ref_feature.y,
|
||||
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
|
||||
ref_feature.y,
|
||||
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
|
||||
|
||||
// alfa = 0, betha = 15 degree - 0.261799 radian
|
||||
//return (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.y)+(float)(sin(0.261799)*cos(cur.z)*ref_feature.z)),
|
||||
// ((float)(cos(cur.z)*ref_feature.y)-(float)(sin(cur.z)*ref_feature.z)),
|
||||
// ((float)(-1*sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*sin(cur.z)*ref_feature.y)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.z)));
|
||||
//
|
||||
// betha = 0, alfa = 15 degree - 0.261799 radian
|
||||
//return (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(-1*sin(0.261799)*cos(cur.z)*ref_feature.y)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.z)),
|
||||
// ((float)(sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.y)-(float)(cos(0.261799)*sin(cur.z)*ref_feature.z)),
|
||||
// ((float)(sin(cur.z)*ref_feature.y)+(float)(cos(cur.z)*ref_feature.z)));
|
||||
}
|
||||
|
||||
void jacobian_xyz2uv(float3 xyz_in_f, float* J)
|
||||
|
||||
@@ -170,13 +170,17 @@ 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_);
|
||||
assert (false);
|
||||
/*
|
||||
//assert (false);
|
||||
|
||||
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
|
||||
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
|
||||
<<"\t distance between two frames: "<<
|
||||
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/
|
||||
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
|
||||
if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
|
||||
size_t sfba_n_edges_final=0;
|
||||
double sfba_thresh, sfba_error_init, sfba_error_final;
|
||||
|
||||
+221
-200
@@ -27,11 +27,12 @@
|
||||
#include <vio/abstract_camera.h>
|
||||
#include <vio/math_utils.h>
|
||||
#include <vio/timer.h>
|
||||
#include <fstream>
|
||||
|
||||
namespace vio {
|
||||
|
||||
Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map, opencl* kernel) :
|
||||
map_(map), gpu_freak(kernel)
|
||||
Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map/*, opencl* kernel*/) :
|
||||
map_(map)//, gpu_freak(kernel)
|
||||
{
|
||||
initializeGrid(cam);
|
||||
}
|
||||
@@ -131,43 +132,62 @@ void Reprojector::reprojectMap(
|
||||
// At the end, we should have at maximum one reprojected point per cell.
|
||||
//SVO_START_TIMER("feature_align");
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
std::vector<cl_float4> 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++;
|
||||
}
|
||||
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;
|
||||
}
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/*cl_float3 keypoints[overlap_kfs.back().second];
|
||||
size_t counter = 0;
|
||||
for(size_t i = 0; i < grid_.cells.size(); ++i)
|
||||
{
|
||||
Cell* cell = grid_.cells.at(grid_.cell_order[i]);
|
||||
for(auto it = cell->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
|
||||
}
|
||||
}*/
|
||||
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// std::vector<cl_float4> 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<<ftr.x<<","<<ftr.y<<","<<ftr.z<<std::endl;
|
||||
// }
|
||||
// fileref.close();
|
||||
|
||||
|
||||
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// ofstream filecur;
|
||||
// filecur.open("/root/Projects/ROS/src/p_33_vio/GPU_version/datacur.txt");
|
||||
// //cl_float3 keypoints[overlap_kfs.back().second];
|
||||
// size_t counter = 0;
|
||||
// for(size_t i = 0; i < grid_.cells.size(); ++i)
|
||||
// {
|
||||
// Cell* cell = grid_.cells.at(grid_.cell_order[i]);
|
||||
// for(auto it = cell->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<<it->px.x()<<","<<it->px.y()<<","<<0.0<<std::endl;
|
||||
// }
|
||||
// }
|
||||
// filecur.close();
|
||||
|
||||
|
||||
// opencl_initializer();
|
||||
// cv::Mat img = overlap_kfs.back().first->img();
|
||||
// cv::Mat integral_img=cv::Mat(cv::Size(701, 501),CV_8UC1);
|
||||
@@ -175,156 +195,156 @@ void Reprojector::reprojectMap(
|
||||
// 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];
|
||||
// // 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);
|
||||
@@ -338,19 +358,20 @@ void Reprojector::reprojectMap(
|
||||
// gpu_freak->reload_buf(1,9,kpScaleIdx);
|
||||
// gpu_freak->run(1);
|
||||
// cl_uchar descriptor[frame_number*300*64]={0};
|
||||
// gpu_freak->read(1,3,300*64,descriptor);
|
||||
// gpu_freak->read(1,3,frame_number*300*64,descriptor);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// for(size_t i=0; i<grid_.cells.size(); ++i)
|
||||
// {
|
||||
// // we prefer good quality points over unkown quality (more likely to match)
|
||||
// // and unknown quality over candidates (position not optimized)
|
||||
// if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
|
||||
// ++n_matches_;
|
||||
// if(n_matches_ > (size_t) Config::maxFts())
|
||||
// break;
|
||||
// }
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
for(size_t i=0; i<grid_.cells.size(); ++i)
|
||||
{
|
||||
// we prefer good quality points over unkown quality (more likely to match)
|
||||
// and unknown quality over candidates (position not optimized)
|
||||
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
|
||||
++n_matches_;
|
||||
if(n_matches_ > (size_t) Config::maxFts())
|
||||
break;
|
||||
}
|
||||
// SVO_STOP_TIMER("feature_align");
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user