Skip to content
Snippets Groups Projects
Commit 04c07906 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

trying to add quickharris

parent de72f2ab
No related branches found
No related tags found
No related merge requests found
#include "../quickharris/detector_quickharris.h"
namespace vision_utils {
QuickHarrisDetector::QuickHarrisDetector(
int _convolutionBoxSize, double _threshold, double _edge
#if GAUSSIAN_MASK_APPROX
, int _convolutionGaussianApproxCoeffsNumber
#endif
) : m_derivationSize_(1), m_convolutionSize_(_convolutionBoxSize), m_threshold_(_threshold), m_edge_(_edge), m_quickData_(nullptr)
{
shift_conv_ = (m_convolutionSize_ - 1) / 2;
#if GAUSSIAN_MASK_APPROX
m_nConvCoeffs = _convolutionGaussianApproxCoeffsNumber;
shift_convs_ = new int[m_nConvCoeffs];
for(int i = 0; i < m_nConvCoeffs; ++i)
shift_convs_[i] = (shift_conv_+1) * (m_nConvCoeffs-i) / m_nConvCoeffs - 1;
nConvCoeffs_ = m_nConvCoeffs;
nConvCoeffsCenter_ = false;
if (shift_convs_[m_nConvCoeffs-1] == 0)
{ nConvCoeffs_--; nConvCoeffsCenter_ = true; }
// compute convolution coeffs
convCoeffs_ = new int[m_nConvCoeffs];
double *sumConvCoeffs = new double[m_nConvCoeffs];
for(int k = 0; k < m_nConvCoeffs; ++k) sumConvCoeffs[k] = 0.0;
normCoeff_ = 0.0;
if (m_nConvCoeffs == 1)
{ convCoeffs_[0] = 1; normCoeff_ = sqrt(shift_conv_*2+1); }
else
{
double sigma = (shift_conv_+0.5) / 2.0; // we want 2.0 * sigma on the border of the mask (95%)
for(int i = 0; i <= shift_conv_; ++i) for(int j = 0; j <= shift_conv_; ++j) for(int k = 0; k < m_nConvCoeffs; ++k)
{
if (k != m_nConvCoeffs-1 && (i <= shift_convs_[k+1] && j <= shift_convs_[k+1])) continue;
int nUse = 1; if (i != 0) nUse *= 2; if (j != 0) nUse *= 2;
sumConvCoeffs[k] += nUse * jmath::evalGaussian(sigma, i, j);
//std::cout << "pix " << i << "," << j << " in " << k << " used " << nUse << " times has coeff " << evalGaussian(sigma, i, j) << std::endl;
break;
}
int lastConvCoeff = 0, lastConvCoeff_tmp;
for(int k = 0; k < m_nConvCoeffs; ++k)
{
int npix = sqrt(shift_convs_[k]*2+1);
if (k != m_nConvCoeffs-1) npix -= sqrt(shift_convs_[k+1]*2+1);
sumConvCoeffs[k] /= npix;
// quantify
convCoeffs_[k] = std::round(sumConvCoeffs[k]*1024);
//std::cout << "*k " << k << " shift " << shift_convs_[k] << " sum " << sumConvCoeffs[k]*npix << " npix " << npix << " coeff " << convCoeffs_[k];
normCoeff_ += convCoeffs_[k] * npix;
lastConvCoeff_tmp = convCoeffs_[k];
convCoeffs_[k] -= lastConvCoeff;
lastConvCoeff = lastConvCoeff_tmp;
//std::cout << " compensated " << convCoeffs_[k] << std::endl;
}
}
delete[] sumConvCoeffs;
int_upLeft_ = new HQuickData*[nConvCoeffs_];
int_upRight_ = new HQuickData*[nConvCoeffs_];
int_downLeft_ = new HQuickData*[nConvCoeffs_];
int_downRight_ = new HQuickData*[nConvCoeffs_];
#else
normCoeff_ = sqrt(shift_conv_*2+1);
#endif
}
QuickHarrisDetector::~QuickHarrisDetector()
{
#if GAUSSIAN_MASK_APPROX
delete[] shift_convs_;
delete[] convCoeffs_;
delete[] int_upLeft_;
delete[] int_upRight_;
delete[] int_downLeft_;
delete[] int_downRight_;
#endif
}
bool QuickHarrisDetector::detect(const cv::Mat& _image, cv::KeyPoint& _kp, double& _scoreBest, const cv::Rect& _roi)
{
cv::Rect localRoi;
if (_roi.width == 0 || _roi.height == 0)
localRoi = cv::Rect(0,0,_image.cols,_image.rows);
else
localRoi = _roi;
int pixBest[2];
double scoreBest;
m_quickData_ = new HQuickData[localRoi.width * localRoi.height]; // processing data structure with integral convolution images
quickDerivatives(_image, localRoi);
bool success = quickConvolutionWithBestPoint(localRoi, pixBest, scoreBest);
delete[] m_quickData_;
if (success)
{
_kp = cv::KeyPoint(pixBest[0]+0.5, pixBest[1]+0.5, CV_32F);
_scoreBest = scoreBest;
}
//std::cout << "Harris [" << scoreBest << "]: (" << pixBest[0] << "," << pixBest[1] << ")" << std::endl;
return success;
}
void QuickHarrisDetector::quickDerivatives( const cv::Mat& _image, cv::Rect& _roi)
{
const uchar* pix_center = nullptr;
const uchar* pix_right = nullptr;
const uchar* pix_left = nullptr;
const uchar* pix_down = nullptr;
const uchar* pix_up = nullptr;
HQuickData* int_center = nullptr;
HQuickData* int_up = nullptr;
HQuickData* int_upLeft = nullptr;
HQuickData* int_left = nullptr;
// dead zone due to derivative mask [-1 0 1]
int shift_derv = 1; // = (3-1)/2
// i: vert; j: horz image coordinates
int iMin = _roi.y + shift_derv;
int iMax = _roi.y + _roi.height - shift_derv;
// FIXME take into account possible convex part of roi x(i) and w(i) by moving this into the loop
int jMin = _roi.x + shift_derv;
int jMax = _roi.x + _roi.width - shift_derv;
int i, j; // i, j: image coordinates
int ri; // ri: vertical roi coordinate
for (i = iMin; i < iMax; i++) {
ri = i - _roi.y;
pix_center = _image.data + (i * _image.step) + jMin;
pix_right = pix_center + 1;
pix_left = pix_center - 1;
pix_down = pix_center + _image.step;
pix_up = pix_center - _image.step;
int_center = m_quickData_ + (ri * _roi.width) + shift_derv;
int_up = int_center - _roi.width;
int_upLeft = int_up - 1;
int_left = int_center - 1;
for (j = jMin; j < jMax; j++) {
// Build x and y derivatives, and their products: xx, xy and yy.
int_center->im_x = *pix_right - *pix_left;
int_center->im_y = *pix_down - *pix_up;
int_center->im_xx = int_center->im_x * int_center->im_x;
int_center->im_xy = int_center->im_x * int_center->im_y;
int_center->im_yy = int_center->im_y * int_center->im_y;
// build integral images of the 3 derivative products, xx, xy and yy:
if (i == iMin && j == jMin) {
int_center->int_xx = int_center->im_xx;
int_center->int_xy = int_center->im_xy;
int_center->int_yy = int_center->im_yy;
} else if (i == iMin) {
int_center->int_xx = int_center->im_xx + int_left->int_xx;
int_center->int_xy = int_center->im_xy + int_left->int_xy;
int_center->int_yy = int_center->im_yy + int_left->int_yy;
} else if (j == jMin) {
int_center->int_xx = int_center->im_xx + int_up->int_xx;
int_center->int_xy = int_center->im_xy + int_up->int_xy;
int_center->int_yy = int_center->im_yy + int_up->int_yy;
} else {
int_center->int_xx = int_center->im_xx + int_up->int_xx
+ int_left->int_xx - int_upLeft->int_xx;
int_center->int_xy = int_center->im_xy + int_up->int_xy
+ int_left->int_xy - int_upLeft->int_xy;
int_center->int_yy = int_center->im_yy + int_up->int_yy
+ int_left->int_yy - int_upLeft->int_yy;
}
// Advance all pointers
pix_center++;
pix_right++;
pix_left++;
pix_up++;
pix_down++;
int_center++;
int_up++;
int_left++;
int_upLeft++;
}
}
}
bool QuickHarrisDetector::quickConvolutionWithBestPoint(const cv::Rect& _roi, int pixMax[2], double& scoreMax)
{
int shift_derv = 1;
int shift_derv_conv = shift_derv + shift_conv_;
// bounds
int riMin = shift_derv_conv;
int riMax = _roi.height - shift_derv_conv;
int rjMin = shift_derv_conv;
int rjMax = _roi.width - shift_derv_conv;
// data structure pointers
HQuickData* int_center;
#if GAUSSIAN_MASK_APPROX
#else
HQuickData* int_upLeft = nullptr;
HQuickData* int_upRight = nullptr;
HQuickData* int_downLeft = nullptr;
HQuickData* int_downRight = nullptr;
#endif
double im_low_curv_max = 0.; // maximum smallest eigenvalue
// DEBUG
// double im_high_curv_max = 0.;
int sm, df; // sum and difference
double sr, corner_ratio;
int ri, rj; // roi coordinates
for (ri = riMin; ri < riMax; ri++) {
int_center = m_quickData_ + (ri * _roi.width) + rjMin;
#if GAUSSIAN_MASK_APPROX
for(int i = 0; i < nConvCoeffs_; ++i)
{
int_upLeft_[i] = int_center - (shift_convs_[i] * (roi.width+1));
int_upRight_[i] = int_center - (shift_convs_[i] * (roi.width-1));
int_downLeft_[i] = int_center + (shift_convs_[i] * (roi.width-1));
int_downRight_[i] = int_center + (shift_convs_[i] * (roi.width+1));
}
#else
int_upLeft = int_center - (shift_conv_ * _roi.width) - shift_conv_;
int_upRight = int_center - (shift_conv_ * _roi.width) + shift_conv_;
int_downLeft = int_center + (shift_conv_ * _roi.width) - shift_conv_;
int_downRight = int_center + (shift_conv_ * _roi.width) + shift_conv_;
#endif
for (rj = rjMin; rj < rjMax; rj++) {
#if GAUSSIAN_MASK_APPROX
int_center->im_conv_xx = int_center->im_conv_xy = int_center->im_conv_yy = 0.;
for(int i = 0; i < nConvCoeffs_; ++i)
{
int_center->im_conv_xx += convCoeffs_[i]*(
int_downRight_[i]->int_xx - int_upRight_[i]->int_xx -
int_downLeft_[i]->int_xx + int_upLeft_[i]->int_xx);
int_center->im_conv_xy += convCoeffs_[i]*(
int_downRight_[i]->int_xy - int_upRight_[i]->int_xy -
int_downLeft_[i]->int_xy + int_upLeft_[i]->int_xy);
int_center->im_conv_yy += convCoeffs_[i]*(
int_downRight_[i]->int_yy - int_upRight_[i]->int_yy -
int_downLeft_[i]->int_yy + int_upLeft_[i]->int_yy);
}
if (nConvCoeffsCenter_)
{
int_center->im_conv_xx += convCoeffs_[nConvCoeffs_]*int_center->im_xx;
int_center->im_conv_xy += convCoeffs_[nConvCoeffs_]*int_center->im_xy;
int_center->im_conv_yy += convCoeffs_[nConvCoeffs_]*int_center->im_yy;
}
#else
int_center->im_conv_xx = int_downRight->int_xx - int_upRight->int_xx
- int_downLeft->int_xx + int_upLeft->int_xx;
int_center->im_conv_xy = int_downRight->int_xy - int_upRight->int_xy
- int_downLeft->int_xy + int_upLeft->int_xy;
int_center->im_conv_yy = int_downRight->int_yy - int_upRight->int_yy
- int_downLeft->int_yy + int_upLeft->int_yy;
#endif
// get eigenvalues: EIG/eig = I_xx + I_yy +/- sqrt((Ixx - I_yy)^2 + 4*I_xy^2)
sm = int_center->im_conv_xx + int_center->im_conv_yy;
df = int_center->im_conv_xx - int_center->im_conv_yy;
sr = sqrt((double) ((double) df * (double) df + 4
* ((double) int_center->im_conv_xy)
* ((double) int_center->im_conv_xy)));
int_center->im_high_curv = (double) sm + sr; // Smallest eigenvalue.
int_center->im_low_curv = (double) sm - sr; // Largest eigenvalue.
// detect and write pixel corresponding to strongest corner, with score.
corner_ratio = int_center->im_high_curv / int_center->im_low_curv; // CAUTION should be high/low
if (corner_ratio < m_edge_) {
if (int_center->im_low_curv > im_low_curv_max) {
im_low_curv_max = int_center->im_low_curv;
//im_high_curv_max = int_center->im_high_curv; // DEBUG
pixMax[0] = _roi.x + rj;
pixMax[1] = _roi.y + ri;
}
}
int_center++;
#if GAUSSIAN_MASK_APPROX
for(int i = 0; i < nConvCoeffs_; ++i)
{
int_upLeft_[i]++;
int_upRight_[i]++;
int_downLeft_[i]++;
int_downRight_[i]++;
}
#else
int_upLeft++;
int_upRight++;
int_downLeft++;
int_downRight++;
#endif
}
}
// normalized score: over the size of the derivative and convolution windows
scoreMax = sqrt(im_low_curv_max / normCoeff_);
//std::cout << "- at " << pixMax[0] << "," << pixMax[1] << " : high " << im_high_curv_max << " low " << im_low_curv_max << " ; ratio " << im_high_curv_max/im_low_curv_max << " score " << scoreMax << std::endl;
#if FILTER_VIRTUAL_POINTS
/*
accept:
_|_ (4) |_ _/ _ (2) _ _ _ (1 or 2)
| \ \ /
reject:
|_ (3) . (0)
|
*/
int_center = m_quickData_ + ((pixMax[1]-_roi.y) * _roi.width) + (pixMax[0]-_roi.x);
int indexes[8][2] = {{-1,-1},{-1,0},{-1,1},{0,1},{1,1},{1,0},{1,-1},{0,-1}};
int radius = m_convolutionSize_/2;
for(int i = 0; i < 8; ++i) for(int j = 0; j < 2; ++j) indexes[i][j] *= radius;
double thres = im_low_curv_max * 0.75;
int npeaks = 0;
bool lastpeaked = false;
bool firstpeaked = false;
for(int i = 0; i < 8; ++i)
{
int_upLeft_ = int_center + indexes[i][1] * _roi.width + indexes[i][0];
if (int_upLeft_->im_high_curv > thres)
{
if (i == 0) firstpeaked = true; else
if (i == 7) lastpeaked = lastpeaked || firstpeaked;
if (!lastpeaked) npeaks++; lastpeaked = true;
} else
lastpeaked = false;
std::cout << " high " << int_upLeft_->im_high_curv << " " << " low " << int_upLeft_->im_low_curv << std::endl;
}
bool ok = (npeaks == 1 || npeaks == 2 || npeaks == 4);
std::cout << " high " << int_center->im_high_curv << " low " << int_center->im_low_curv << " npeaks " << npeaks << " ok " << ok << std::endl;
#endif
return (scoreMax > m_threshold_);
}
/**
* Vision Utils specifics
*/
DetectorQUICKHARRIS::DetectorQUICKHARRIS(void)
{}
DetectorQUICKHARRIS::~DetectorQUICKHARRIS(void)
{}
KeyPointVector DetectorQUICKHARRIS::detect(const cv::Mat& _image, const cv::InputArray& _mask)
{
clock_t tStart = clock();
cv::Mat mask;
if (!_mask.empty())
mask = _mask.getMat();
std::map<int,cv::KeyPoint> kpts_map;
KeyPointVector kpts;
if (!_image.empty())
{
for (int uu=0;uu<_image.cols;++uu)
{
for (int vv=0;vv<_image.rows;++vv)
{
int patterRadius = getPatternRadius();
double score;
cv::KeyPoint kp;
cv::Rect roi(uu,vv,patterRadius,patterRadius);
if (_mask.empty())
{
score = detectSingle(_image, roi, kp);
kpts_map[score] = kp;
}
else
{
if ((int)mask.at<uchar>(uu,vv)==255)
{
score = detectSingle(_image, roi, kp);
kpts_map[score] = kp;
}
}
}
}
// Set max number of KeyPoints
for( std::map<int,cv::KeyPoint>::iterator it = kpts_map.begin(); it != kpts_map.end(); ++it )
if (kpts.size()<max_kpoints_)
kpts.push_back( it->second );
else
break;
}
comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
return kpts;
}
KeyPointVector DetectorQUICKHARRIS::detect(const cv::Mat& _image, cv::Rect& _roi)
{
std::map<int,cv::KeyPoint> kpts_map;
KeyPointVector kpts;
if (!_image.empty())
{
int patterRadius = getPatternRadius();
for (int uu=_roi.x;uu<_roi.x+_roi.width;uu=uu+patterRadius)
{
for (int vv=_roi.y;vv<_roi.y+_roi.width;vv=vv+patterRadius)
{
double score;
cv::KeyPoint kp;
cv::Rect roi(uu,vv,patterRadius,patterRadius);
score = detectSingle(_image, roi, kp);
kpts_map[score] = kp;
}
}
// Set max number of KeyPoints
for( std::map<int,cv::KeyPoint>::iterator it = kpts_map.begin(); it != kpts_map.end(); ++it )
if (kpts.size()<max_kpoints_)
kpts.push_back( it->second );
else
break;
}
return kpts;
}
double DetectorQUICKHARRIS::detectSingle(const cv::Mat& _image, cv::Rect& _roi, cv::KeyPoint& _kp)
{
double scoreBest=0.0;
if (!_image.empty())
detector_->detect(_image, _kp, scoreBest, _roi);
return scoreBest;
}
} /* namespace vision_utils */
// Register in the DetectorsFactory
namespace vision_utils
{
VU_REGISTER_DETECTOR("QUICKHARRIS", DetectorQUICKHARRIS);
} /* namespace vision_utils */
/**
* \file Based on QuickHarrisDetector in RTSLAM project from Joan Solà
*/
#ifndef _DETECTOR_QUICKHARRIS_H_
#define _DETECTOR_QUICKHARRIS_H_
#include "../detector_base.h"
#include "../detector_factory.h"
namespace vision_utils {
/*
* STATUS: in progress, do not use for now
* Approximate a gaussian mask with integral images, in order to
* improve precision (getting points closer to the real corner).
* Disabled for now because it hasn't proved to be really more efficient.
*/
#define GAUSSIAN_MASK_APPROX 0
/*
* STATUS: in progress, do not use for now
* Try to detect virtual corners (created by a depth discontinuity).
* It does not eliminate them yet, we're just trying to find a good criteria,
* the current one tries to quickly count the gradient peaks around the point.
*/
#define FILTER_VIRTUAL_POINTS 0
/**
* Quick Harris detector class.
* \ingroup rtslam
* \author jsola
*
* This class detects the strongest Harris point inside a given region of interest.
*
* The class is called Quick because the algorithm is accelerated using 5 strategies:
* - the use of a simple derivative mask [-1 0 1] that avoids products and minimizes sums.
* - the use of a square convolution mask of fixed amplitude = 1.
* - this mask allows us to compute the convolution via integral images of I_xx, I_xy and I_yy.
* - we extract the best point, so we don't need to perform theresholding and sub-maxima suppression (both expensive).
* - we purposely use small regions of interest.
*
* Because of simplifications 1. 2. and 3., the result is sub-optimal in the sense
* of Harris standards, but it gives strong corner points that can
* be tracked by means of correlation (zncc for example).
*/
// Create all pointers
VU_PTR_TYPEDEFS(QuickHarrisDetector);
class QuickHarrisDetector {
public:
QuickHarrisDetector(int _convolutionBoxSize = 5, double _threshold = 15.0, double _edge = 2.0
#if GAUSSIAN_MASK_APPROX
, int _convolutionGaussianApproxCoeffsNumber = 3
#endif
);
~QuickHarrisDetector();
virtual bool detect(const cv::Mat& _image, cv::KeyPoint& _kp, double& _scoreBest, const cv::Rect& _roi = cv::Rect(0,0,0,0));
private:
int m_derivationSize_;
int m_convolutionSize_;
double m_threshold_;
double m_edge_;
struct HQuickData {
int im_x, im_y, im_xx, im_xy, im_yy, im_conv_xx, im_conv_xy, im_conv_yy;
int int_xx, int_xy, int_yy;
double im_high_curv, im_low_curv;
};
HQuickData* m_quickData_; // integral image for quick detector.
int shift_conv_;
double normCoeff_;
#if GAUSSIAN_MASK_APPROX
int m_nConvCoeffs_;
int *shift_convs_;
int *convCoeffs_;
int nConvCoeffs_;
bool nConvCoeffsCenter_;
HQuickData** int_upLeft_;
HQuickData** int_upRight_;
HQuickData** int_downLeft_;
HQuickData** int_downRight_;
#endif
void quickDerivatives(const cv::Mat& _image, cv::Rect& _roi);
bool quickConvolutionWithBestPoint(const cv::Rect& _roi, int _pixMax[2], double& _scoreMax);
};
/**
* Vision Utils specifics
*/
// Create all pointers
VU_PTR_TYPEDEFS(DetectorQUICKHARRIS);
VU_PTR_TYPEDEFS(DetectorParamsQUICKHARRIS);
/** \brief Class parameters
*
*/
struct DetectorParamsQUICKHARRIS: public DetectorParamsBase
{
int max_kpoints = 500;
int convolutionBoxSize = 5;
double threshold = 15.0;
double edge = 2.0;
#if GAUSSIAN_MASK_APPROX
int convolutionGaussianApproxCoeffsNumber = 3;
#endif
DetectorParamsQUICKHARRIS() : DetectorParamsBase("QUICKHARRIS"){};
~DetectorParamsQUICKHARRIS(){};
};
/** \brief DETECTOR class
*
*/
class DetectorQUICKHARRIS : public DetectorBase {
public:
DetectorQUICKHARRIS();
virtual ~DetectorQUICKHARRIS(void);
KeyPointVector detect(const cv::Mat& _image, const cv::InputArray& _mask=cv::noArray() );
KeyPointVector detect(const cv::Mat& _image, cv::Rect& _roi);
double detectSingle(const cv::Mat& _image, cv::Rect& _roi, cv::KeyPoint& _kp);
// Factory method
static DetectorBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);
private:
QuickHarrisDetectorPtr detector_;
int max_kpoints_;
void defineDetector(const ParamsBasePtr _params);
};
/*
* brief Define detector
*/
inline void DetectorQUICKHARRIS::defineDetector(const ParamsBasePtr _params)
{
DetectorParamsQUICKHARRISPtr params_ptr = std::static_pointer_cast<DetectorParamsQUICKHARRIS>(_params);
#if GAUSSIAN_MASK_APPROX
int convolutionGaussianApproxCoeffsNumber = 3;
#endif
detector_ = std::make_shared<QuickHarrisDetector>(params_ptr->convolutionBoxSize,
params_ptr->threshold,
params_ptr->edge
#if GAUSSIAN_MASK_APPROX
, params_ptr->convolutionGaussianApproxCoeffsNumber
#endif
);
pattern_radius_ = params_ptr->convolutionBoxSize+params_ptr->edge;
max_kpoints_ = params_ptr->max_kpoints;
}
/*
* brief Create object in factory
*/
inline DetectorBasePtr DetectorQUICKHARRIS::create(const std::string& _unique_name, const ParamsBasePtr _params)
{
DetectorQUICKHARRISPtr det_ptr = std::make_shared<DetectorQUICKHARRIS>();
det_ptr->setName(_unique_name);
det_ptr->defineDetector(_params);
return det_ptr;
}
} /* namespace vision_utils */
#endif /* _DETECTOR_QUICKHARRIS_H_ */
#include "../quickharris/detector_quickharris.h"
#ifdef USING_YAML
// yaml-cpp library
#include <yaml-cpp/yaml.h>
namespace vision_utils
{
namespace
{
static ParamsBasePtr createParamsQUICKHARRISDetector(const std::string & _filename_dot_yaml)
{
DetectorParamsQUICKHARRISPtr params_ptr = std::make_shared<DetectorParamsQUICKHARRIS>();
using std::string;
using YAML::Node;
Node yaml_params = YAML::LoadFile(_filename_dot_yaml);
if (!yaml_params.IsNull())
{
Node d_yaml = yaml_params["detector"];
if(d_yaml["type"].as<string>() == "QUICKHARRIS")
{
params_ptr->max_kpoints = d_yaml["max_kpoints"].as<int>();
params_ptr->convolutionBoxSize = d_yaml["convolutionBoxSize"].as<int>();
params_ptr->threshold = d_yaml["threshold"].as<double>();
params_ptr->edge = d_yaml["edge"].as<double>();
}else
{
std::cerr << "Bad configuration file. Wrong type " << d_yaml["type"].as<string>() << std::endl;
return nullptr;
}
}
return params_ptr;
}
// Register in the SensorFactory
const bool VU_UNUSED registered_detQUICKHARRIS_params = ParamsFactory::get().registerCreator("QUICKHARRIS DET", createParamsQUICKHARRISDetector);
} /* namespace [unnamed] */
} /* namespace vision_utils */
#endif /* IF USING_YAML */
sensor:
type: "USB_CAM"
detector:
type: "QUICKHARRIS"
max_kpoints: 200
convolutionBoxSize: 5
threshold: 15.0
edge: 2.0
\ No newline at end of file
......@@ -310,14 +310,26 @@ TEST(TrifocalTensor, ComputeTensorSyntheticWNoise)
list3.row(8) << 247.6911, 244.6778, 1.0;
std::srand((unsigned int) time(0));
Eigen::MatrixXd Noise1 = 0.2*Eigen::MatrixXd::Random(9,2);
Eigen::MatrixXd Noise2 = 0.2*Eigen::MatrixXd::Random(9,2);
Eigen::MatrixXd Noise3 = 0.2*Eigen::MatrixXd::Random(9,2);
Eigen::MatrixXd Noise1 = Eigen::MatrixXd::Random(9,2);
Eigen::MatrixXd Noise2 = Eigen::MatrixXd::Random(9,2);
Eigen::MatrixXd Noise3 = Eigen::MatrixXd::Random(9,2);
// Evaluation (without noise)
Eigen::MatrixXd l1_eval(list1), l2_eval(list2), l3_eval(list3);
vision_utils::normalizePointsIsotrop(list1,l1_eval);
vision_utils::normalizePointsIsotrop(list2,l2_eval);
vision_utils::normalizePointsIsotrop(list3,l3_eval);
// Tensor computation (with noise)
list1.block(0,0,9,2) = list1.block(0,0,9,2) + Noise1;
list2.block(0,0,9,2) = list2.block(0,0,9,2) + Noise2;
list3.block(0,0,9,2) = list3.block(0,0,9,2) + Noise3;
Eigen::MatrixXd l1_noise(list1), l2_noise(list2), l3_noise(list3);
vision_utils::normalizePointsIsotrop(list1,l1_noise);
vision_utils::normalizePointsIsotrop(list2,l2_noise);
vision_utils::normalizePointsIsotrop(list3,l3_noise);
// Tensor parameters
int max_iterations = 100;
double max_error_reprojection = 1.0;
......@@ -325,10 +337,10 @@ TEST(TrifocalTensor, ComputeTensorSyntheticWNoise)
vision_utils::TrifocalTensor tensor(max_iterations,max_error_reprojection,percentage_of_correct_reprojected_points);
bool tensor_ok = tensor.computeTensor(list1, list2, list3);
bool tensor_ok = tensor.computeTensor(l1_noise, l2_noise, l3_noise);
ASSERT_TRUE(tensor_ok);
double percentage_correct = tensor.getErrorTrifocal(list1, list2, list3);
double percentage_correct = tensor.getErrorTrifocal(l1_eval, l2_eval, l3_eval);
ASSERT_TRUE(percentage_correct>=percentage_of_correct_reprojected_points);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment