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
|
/usr/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.h
|
||||||
CMakeFiles/vio.dir/src/reprojector.cpp.o
|
CMakeFiles/vio.dir/src/reprojector.cpp.o
|
||||||
../include/vio/abstract_camera.h
|
../include/vio/abstract_camera.h
|
||||||
|
../include/vio/cl.hpp
|
||||||
|
../include/vio/cl_class.h
|
||||||
../include/vio/config.h
|
../include/vio/config.h
|
||||||
../include/vio/feature.h
|
../include/vio/feature.h
|
||||||
../include/vio/frame.h
|
../include/vio/frame.h
|
||||||
../include/vio/global.h
|
../include/vio/global.h
|
||||||
|
../include/vio/initialization.h
|
||||||
../include/vio/map.h
|
../include/vio/map.h
|
||||||
../include/vio/matcher.h
|
../include/vio/matcher.h
|
||||||
../include/vio/math_utils.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/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/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/config.h
|
||||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/feature.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/frame.h
|
||||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/global.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/map.h
|
||||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/matcher.h
|
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/matcher.h
|
||||||
CMakeFiles/vio.dir/src/reprojector.cpp.o: ../include/vio/math_utils.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_matches_;
|
||||||
size_t n_trials_;
|
size_t n_trials_;
|
||||||
|
|
||||||
Reprojector(vk::AbstractCamera* cam, Map& map, opencl* kernel);
|
Reprojector(vk::AbstractCamera* cam, Map& map/*, opencl* kernel*/);
|
||||||
|
|
||||||
~Reprojector();
|
~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 r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
|
||||||
float factor = 1.0;
|
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));
|
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);
|
return (float2)((float)C_X + (float)F_X * factor * feature.x/feature.z, (float)C_Y + (float)F_Y * factor * feature.y/feature.z);
|
||||||
}
|
}
|
||||||
@@ -24,6 +24,16 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
|
|||||||
return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - 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,
|
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));
|
-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)
|
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);
|
//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_);
|
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;
|
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_, overlap_kfs_);
|
||||||
assert (false);
|
//assert (false);
|
||||||
/*
|
|
||||||
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
|
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
|
||||||
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
|
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
|
||||||
<<"\t distance between two frames: "<<
|
<<"\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;
|
if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
|
||||||
size_t sfba_n_edges_final=0;
|
size_t sfba_n_edges_final=0;
|
||||||
double sfba_thresh, sfba_error_init, sfba_error_final;
|
double sfba_thresh, sfba_error_init, sfba_error_final;
|
||||||
|
|||||||
+221
-200
@@ -27,11 +27,12 @@
|
|||||||
#include <vio/abstract_camera.h>
|
#include <vio/abstract_camera.h>
|
||||||
#include <vio/math_utils.h>
|
#include <vio/math_utils.h>
|
||||||
#include <vio/timer.h>
|
#include <vio/timer.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
namespace vio {
|
namespace vio {
|
||||||
|
|
||||||
Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map, opencl* kernel) :
|
Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map/*, opencl* kernel*/) :
|
||||||
map_(map), gpu_freak(kernel)
|
map_(map)//, gpu_freak(kernel)
|
||||||
{
|
{
|
||||||
initializeGrid(cam);
|
initializeGrid(cam);
|
||||||
}
|
}
|
||||||
@@ -131,43 +132,62 @@ void Reprojector::reprojectMap(
|
|||||||
// At the end, we should have at maximum one reprojected point per cell.
|
// At the end, we should have at maximum one reprojected point per cell.
|
||||||
//SVO_START_TIMER("feature_align");
|
//SVO_START_TIMER("feature_align");
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
std::vector<cl_float4> kps; // keypoints
|
// std::vector<cl_float4> kps; // keypoints
|
||||||
cl_float4 data;
|
// cl_float4 data;
|
||||||
int frame_number = 0;
|
// 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_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++){
|
// 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.w = frame_number;
|
||||||
data.x = (*it_ftr)->px.x();
|
// data.x = (*it_ftr)->px.x();
|
||||||
data.y = (*it_ftr)->px.y();
|
// data.y = (*it_ftr)->px.y();
|
||||||
data.z = 0.0;
|
// data.z = 0.0;
|
||||||
kps.push_back(data);
|
// kps.push_back(data);
|
||||||
}
|
// }
|
||||||
frame_number++;
|
// frame_number++;
|
||||||
}
|
// }
|
||||||
cl_float4 keypoints[kps.size()];
|
|
||||||
size_t counter = 0;
|
|
||||||
for(auto it = kps.begin(), ite = kps.end(); it != ite; it++) {
|
// cv::Mat imgbegin = overlap_kfs.begin()->first->img().clone();
|
||||||
cl_float4 ftr;
|
// cv::imwrite("/root/Projects/ROS/src/p_33_vio/GPU_version/img_overlap_kfs_begin.png",imgbegin);
|
||||||
ftr.w = it->w;
|
// cv::Mat imgbend = overlap_kfs.end()->first->img().clone();
|
||||||
ftr.x = it->x;
|
// cv::imwrite("/root/Projects/ROS/src/p_33_vio/GPU_version/img_overlap_kfs_end.png",imgbend);
|
||||||
ftr.y = it->y;
|
|
||||||
ftr.z = it->z;
|
// ofstream fileref;
|
||||||
keypoints[counter++] = ftr;
|
// fileref.open("/root/Projects/ROS/src/p_33_vio/GPU_version/dataref.txt");
|
||||||
}
|
// //cl_float4 keypoints[kps.size()];
|
||||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
// //size_t counter = 0;
|
||||||
/*cl_float3 keypoints[overlap_kfs.back().second];
|
// for(auto it = kps.begin(), ite = kps.end(); it != ite; it++) {
|
||||||
size_t counter = 0;
|
// cl_float4 ftr;
|
||||||
for(size_t i = 0; i < grid_.cells.size(); ++i)
|
// ftr.w = it->w;
|
||||||
{
|
// ftr.x = it->x;
|
||||||
Cell* cell = grid_.cells.at(grid_.cell_order[i]);
|
// ftr.y = it->y;
|
||||||
for(auto it = cell->begin(), ite = cell->end(); it != ite; it++, counter++)
|
// ftr.z = it->z;
|
||||||
{
|
// //keypoints[counter++] = ftr;
|
||||||
keypoints[counter].x = it->px.x(); //x
|
// fileref<<ftr.x<<","<<ftr.y<<","<<ftr.z<<std::endl;
|
||||||
keypoints[counter].y = it->px.y(); //y
|
// }
|
||||||
keypoints[counter].z = 0.0; //angle
|
// 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();
|
// opencl_initializer();
|
||||||
// cv::Mat img = overlap_kfs.back().first->img();
|
// cv::Mat img = overlap_kfs.back().first->img();
|
||||||
// cv::Mat integral_img=cv::Mat(cv::Size(701, 501),CV_8UC1);
|
// 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->run(0);
|
||||||
// gpu_freak->read(0,1,1,&integral_img);
|
// gpu_freak->read(0,1,1,&integral_img);
|
||||||
|
|
||||||
// FREAK preprocessing
|
// // FREAK preprocessing
|
||||||
static const float FREAK_LOG2 = 0.693147180559945;
|
// static const float FREAK_LOG2 = 0.693147180559945;
|
||||||
static const int FREAK_NB_ORIENTATION = 256;
|
// static const int FREAK_NB_ORIENTATION = 256;
|
||||||
static const int NB_ORIENPAIRS = 45;
|
// static const int NB_ORIENPAIRS = 45;
|
||||||
static const int FREAK_NB_POINTS = 43;
|
// static const int FREAK_NB_POINTS = 43;
|
||||||
static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints
|
// static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints
|
||||||
static const int NB_PAIRS = 512;
|
// static const int NB_PAIRS = 512;
|
||||||
static const int NB_SCALES = 64;
|
// static const int NB_SCALES = 64;
|
||||||
|
//
|
||||||
bool orientationNormalized = true;
|
// bool orientationNormalized = true;
|
||||||
bool scaleNormalized = true;
|
// bool scaleNormalized = true;
|
||||||
float patternScale = 22.0f;
|
// float patternScale = 22.0f;
|
||||||
int nOctaves = 4;
|
// int nOctaves = 4;
|
||||||
|
//
|
||||||
bool extAll = false; // true if all pairs need to be extracted for pairs selection
|
// 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)
|
// 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)
|
// 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_uchar2 descriptionPairs[NB_PAIRS];
|
||||||
cl_int4 orientationPairs[NB_ORIENPAIRS];
|
// cl_int4 orientationPairs[NB_ORIENPAIRS];
|
||||||
|
//
|
||||||
// default pairs
|
// // default pairs
|
||||||
static const int FREAK_DEF_PAIRS[NB_PAIRS] = {
|
// static const int FREAK_DEF_PAIRS[NB_PAIRS] = {
|
||||||
404,431,818,511,181,52,311,874,774,543,719,230,417,205,11,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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,
|
// 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
|
// 670,249,36,581,389,605,331,518,442,822
|
||||||
};
|
// };
|
||||||
|
//
|
||||||
float scaleStep = pow(2.0, (float)(nOctaves)/NB_SCALES ); // 2 ^ ( (nOctaves-1) /nbScales)
|
// 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 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 bigR = (float)(2.0 / 3.0); // bigger radius
|
||||||
const float smallR = (float)(2.0 / 24.0); // smaller 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)
|
// 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)
|
// // radii of the concentric cirles (from outer to inner)
|
||||||
const float radius[8] = {bigR , bigR - 6 * unitSpace, bigR - 11 * unitSpace, bigR - 15 * unitSpace,
|
// const float radius[8] = {bigR , bigR - 6 * unitSpace, bigR - 11 * unitSpace, bigR - 15 * unitSpace,
|
||||||
bigR - 18 * unitSpace, bigR - 20 * unitSpace, smallR , 0.0};
|
// 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)
|
// // 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),
|
// 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)};
|
// 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 )
|
// for( int scaleIdx=0; scaleIdx < NB_SCALES; ++scaleIdx )
|
||||||
{
|
// {
|
||||||
patternSizes[scaleIdx] = 0; // proper initialization
|
// patternSizes[scaleIdx] = 0; // proper initialization
|
||||||
float scalingFactor = pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx
|
// float scalingFactor = pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx
|
||||||
|
//
|
||||||
for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx )
|
// for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx )
|
||||||
{
|
// {
|
||||||
int pointIdx = 0;
|
// int pointIdx = 0;
|
||||||
|
//
|
||||||
for( size_t i = 0; i < 8; ++i )
|
// for( size_t i = 0; i < 8; ++i )
|
||||||
{
|
// {
|
||||||
for( int k = 0 ; k < nn[i]; ++k )
|
// for( int k = 0 ; k < nn[i]; ++k )
|
||||||
{
|
// {
|
||||||
float alpha = (float)(k) * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(nn[i])
|
// float alpha = (float)(k) * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(nn[i])
|
||||||
+ ((float)3.141592653589793238462643383279502884197169399375 / nn[i] * ( i % 2 )/*beta*/)
|
// + ((float)3.141592653589793238462643383279502884197169399375 / nn[i] * ( i % 2 )/*beta*/)
|
||||||
+ ((float)(orientationIdx * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(FREAK_NB_ORIENTATION)) /* theta*/);
|
// + ((float)(orientationIdx * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(FREAK_NB_ORIENTATION)) /* theta*/);
|
||||||
|
//
|
||||||
// add the point to the look-up table
|
// // add the point to the look-up table
|
||||||
cl_float3* point = &patternLookup[scaleIdx * FREAK_NB_ORIENTATION * FREAK_NB_POINTS + orientationIdx * FREAK_NB_POINTS + pointIdx];
|
// 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->x = (float)(radius[i] * cos(alpha) * scalingFactor * patternScale); //x
|
||||||
point->y = (float)(radius[i] * sin(alpha) * scalingFactor * patternScale); //y
|
// point->y = (float)(radius[i] * sin(alpha) * scalingFactor * patternScale); //y
|
||||||
point->z = (float)(sigma[i] * scalingFactor * patternScale); //sigma
|
// point->z = (float)(sigma[i] * scalingFactor * patternScale); //sigma
|
||||||
|
//
|
||||||
// adapt the sizeList if necessary
|
// // adapt the sizeList if necessary
|
||||||
const int sizeMax = (int)(ceil((radius[i] + sigma[i]) * scalingFactor * patternScale)) + 1;
|
// const int sizeMax = (int)(ceil((radius[i] + sigma[i]) * scalingFactor * patternScale)) + 1;
|
||||||
if( patternSizes[scaleIdx] < sizeMax )
|
// if( patternSizes[scaleIdx] < sizeMax )
|
||||||
patternSizes[scaleIdx] = sizeMax;
|
// patternSizes[scaleIdx] = sizeMax;
|
||||||
|
//
|
||||||
++pointIdx;
|
// ++pointIdx;
|
||||||
}
|
// }
|
||||||
}
|
// }
|
||||||
}
|
// }
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
// build the list of orientation pairs
|
// // 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[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[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[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[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[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[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[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[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[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[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[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[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[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[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;
|
// 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--; )
|
// for( unsigned m = NB_ORIENPAIRS; m--; )
|
||||||
{
|
// {
|
||||||
float dx = patternLookup[orientationPairs[m].w].x - patternLookup[orientationPairs[m].x].x;
|
// 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 dy = patternLookup[orientationPairs[m].w].y - patternLookup[orientationPairs[m].x].y;
|
||||||
float norm_sq = (dx * dx + dy * dy);
|
// float norm_sq = (dx * dx + dy * dy);
|
||||||
orientationPairs[m].y = round((dx / (norm_sq)) * 4096.0); //weight_dx
|
// orientationPairs[m].y = round((dx / (norm_sq)) * 4096.0); //weight_dx
|
||||||
orientationPairs[m].z = round((dy / (norm_sq)) * 4096.0); //weight_dy
|
// orientationPairs[m].z = round((dy / (norm_sq)) * 4096.0); //weight_dy
|
||||||
}
|
// }
|
||||||
|
//
|
||||||
// build the list of description pairs
|
// // build the list of description pairs
|
||||||
cl_uchar2 allPairs[(int)((FREAK_NB_POINTS * (FREAK_NB_POINTS + 1)) / 2)];
|
// cl_uchar2 allPairs[(int)((FREAK_NB_POINTS * (FREAK_NB_POINTS + 1)) / 2)];
|
||||||
for( int i = 1, idx = 0; i < FREAK_NB_POINTS; ++i )
|
// for( int i = 1, idx = 0; i < FREAK_NB_POINTS; ++i )
|
||||||
{
|
// {
|
||||||
// (generate all the pairs)
|
// // (generate all the pairs)
|
||||||
for( int j = 0; j < i; ++j )
|
// for( int j = 0; j < i; ++j )
|
||||||
{
|
// {
|
||||||
allPairs[idx++] = (cl_uchar2){(uchar)i, (uchar)j};
|
// allPairs[idx++] = (cl_uchar2){(uchar)i, (uchar)j};
|
||||||
}
|
// }
|
||||||
}
|
// }
|
||||||
for( int i = 0; i < NB_PAIRS; ++i )
|
// for( int i = 0; i < NB_PAIRS; ++i )
|
||||||
descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]];
|
// descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]];
|
||||||
|
//
|
||||||
int kpScaleIdx[overlap_kfs.back().second];
|
// int kpScaleIdx[overlap_kfs.back().second];
|
||||||
|
|
||||||
|
|
||||||
// gpu_freak->reload_buf(1,0,img);
|
// gpu_freak->reload_buf(1,0,img);
|
||||||
@@ -338,19 +358,20 @@ void Reprojector::reprojectMap(
|
|||||||
// gpu_freak->reload_buf(1,9,kpScaleIdx);
|
// gpu_freak->reload_buf(1,9,kpScaleIdx);
|
||||||
// gpu_freak->run(1);
|
// gpu_freak->run(1);
|
||||||
// cl_uchar descriptor[frame_number*300*64]={0};
|
// 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)
|
for(size_t i=0; i<grid_.cells.size(); ++i)
|
||||||
// // and unknown quality over candidates (position not optimized)
|
{
|
||||||
// if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
|
// we prefer good quality points over unkown quality (more likely to match)
|
||||||
// ++n_matches_;
|
// and unknown quality over candidates (position not optimized)
|
||||||
// if(n_matches_ > (size_t) Config::maxFts())
|
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
|
||||||
// break;
|
++n_matches_;
|
||||||
// }
|
if(n_matches_ > (size_t) Config::maxFts())
|
||||||
|
break;
|
||||||
|
}
|
||||||
// SVO_STOP_TIMER("feature_align");
|
// SVO_STOP_TIMER("feature_align");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user