提交scripts
This commit is contained in:
hesuicong committed
1 parent
82caf72036
commit
07dabbd186
224 files changed
+130146
No files matched your search
@@ -0,0 +1,668 @@
|
||||
#ifndef __SEACAVE_AUTO_ESTIMATOR_H__
|
||||
#define __SEACAVE_AUTO_ESTIMATOR_H__
|
||||
|
||||
//-------------------
|
||||
// Generic implementation of ACRANSAC
|
||||
//-------------------
|
||||
// The A contrario parametrization have been first explained in [1] and
|
||||
// later extended to generic model estimation in [2] (with a demonstration for
|
||||
// the homography) and extended and use at large scale for Structure from
|
||||
// Motion in [3].
|
||||
//
|
||||
//--
|
||||
// [1] Lionel Moisan, Berenger Stival,
|
||||
// A probalistic criterion to detect rigid point matches between
|
||||
// two images and estimate the fundamental matrix.
|
||||
// IJCV 04.
|
||||
//--
|
||||
// [2] Lionel Moisan, Pierre Moulon, Pascal Monasse.
|
||||
// Automatic Homographic Registration of a Pair of Images,
|
||||
// with A Contrario Elimination of Outliers
|
||||
// Image Processing On Line (IPOL), 2012.
|
||||
// http://dx.doi.org/10.5201/ipol.2012.mmm-oh
|
||||
//--
|
||||
// [3] Pierre Moulon, Pascal Monasse and Renaud Marlet.
|
||||
// Adaptive Structure from Motion with a contrario mode estimation.
|
||||
// In 11th Asian Conference on Computer Vision (ACCV 2012)
|
||||
//--
|
||||
// [4] cDc@seacave - rewrite it
|
||||
//
|
||||
|
||||
|
||||
// I N C L U D E S /////////////////////////////////////////////////
|
||||
|
||||
|
||||
// D E F I N E S ///////////////////////////////////////////////////
|
||||
|
||||
// uncomment it to enable std::vector implementation
|
||||
// (slower)
|
||||
//#define ACRANSAC_STD_VECTOR
|
||||
|
||||
// uncomment it to enable sampling from inliers
|
||||
// (sometimes does not find the right solution)
|
||||
//#define ACRANSAC_SAMPLE_INLIERS
|
||||
|
||||
|
||||
// P R O T O T Y P E S /////////////////////////////////////////////
|
||||
|
||||
namespace SEACAVE {
|
||||
|
||||
/// logarithm (base 10) of binomial coefficient
|
||||
static inline double logcombi(size_t k, size_t n)
|
||||
{
|
||||
if (k>=n) return(0.0);
|
||||
if (n-k<k) k=n-k;
|
||||
double r = 0.0;
|
||||
for (size_t i = 1; i <= k; i++)
|
||||
r += log10((double)(n-i+1))-log10((double)i);
|
||||
return r;
|
||||
}
|
||||
|
||||
/// tabulate logcombi(.,n)
|
||||
template<typename Type>
|
||||
static inline void makelogcombi_n(size_t n, std::vector<Type>& l)
|
||||
{
|
||||
l.resize(n+1);
|
||||
for (size_t k = 0; k <= n; ++k)
|
||||
l[k] = static_cast<Type>(logcombi(k,n));
|
||||
}
|
||||
|
||||
/// tabulate logcombi(k,.)
|
||||
template<typename Type, int k>
|
||||
static inline void makelogcombi_k(size_t nmax, std::vector<Type>& l)
|
||||
{
|
||||
l.resize(nmax+1);
|
||||
for (size_t n = 0; n <= nmax; ++n)
|
||||
l[n] = static_cast<Type>(logcombi(k,n));
|
||||
}
|
||||
|
||||
/// Distance and associated index
|
||||
struct ErrorIndex {
|
||||
double first;
|
||||
size_t second;
|
||||
inline ErrorIndex() {}
|
||||
inline ErrorIndex(double _first, size_t _second) : first(_first), second(_second) {}
|
||||
inline bool operator==(const ErrorIndex& r) const { return (first == r.first); }
|
||||
inline bool operator <(const ErrorIndex& r) const { return (first < r.first); }
|
||||
};
|
||||
#ifdef ACRANSAC_STD_VECTOR
|
||||
typedef std::vector<ErrorIndex> ErrorIndexArr;
|
||||
#else
|
||||
typedef CLISTDEF0(ErrorIndex) ErrorIndexArr;
|
||||
#endif
|
||||
|
||||
/// Find best NFA and its index wrt square error threshold in e.
|
||||
template<size_t NSAMPLES>
|
||||
static ErrorIndex bestNFA(
|
||||
double logalpha0,
|
||||
const ErrorIndexArr& e,
|
||||
double loge0,
|
||||
double maxThresholdSq,
|
||||
const std::vector<float>& logc_n,
|
||||
const std::vector<float>& logc_k,
|
||||
double multError = 1.0)
|
||||
{
|
||||
ErrorIndex bestIndex(DBL_MAX, NSAMPLES);
|
||||
const size_t n = e.size();
|
||||
for (size_t k=NSAMPLES+1; k<=n && e[k-1].first<=maxThresholdSq; ++k) {
|
||||
const double logalpha = logalpha0 + multError * log10(e[k-1].first+FLT_EPSILON);
|
||||
const double NFA = loge0 +
|
||||
logalpha * (double)(k-NSAMPLES) +
|
||||
logc_n[k] +
|
||||
logc_k[k];
|
||||
if (NFA < bestIndex.first)
|
||||
bestIndex = ErrorIndex(NFA, k);
|
||||
}
|
||||
return bestIndex;
|
||||
}
|
||||
|
||||
/// Pick a random subset of the integers [0, total), in random order.
|
||||
/// Note that this can behave badly if num_samples is close to total; runtime
|
||||
/// could be unlimited!
|
||||
///
|
||||
/// This uses a quadratic rejection strategy and should only be used for small
|
||||
/// num_samples.
|
||||
///
|
||||
/// \param NSAMPLES The number of samples to produce.
|
||||
/// \param n The number of samples available.
|
||||
/// \param samples num_samples of numbers in [0, total_samples) is placed
|
||||
/// here on return.
|
||||
struct UniformSampler {
|
||||
template <size_t NSAMPLES>
|
||||
inline void Sample(
|
||||
size_t n,
|
||||
size_t* samples)
|
||||
{
|
||||
ASSERT(NSAMPLES > 0);
|
||||
size_t* const samplesBegin = samples;
|
||||
size_t* const samplesEnd = samples + NSAMPLES;
|
||||
*samples = size_t(RAND() % n);
|
||||
while (++samples < samplesEnd) {
|
||||
do {
|
||||
*samples = size_t(RAND() % n);
|
||||
} while (std::find(samplesBegin, samples, *samples) != samples);
|
||||
}
|
||||
}
|
||||
};
|
||||
/// Same as above, but ensure that the first point is always included
|
||||
struct UniformSamplerLockFirst {
|
||||
template <size_t NSAMPLES>
|
||||
inline void Sample(
|
||||
size_t n,
|
||||
size_t* samples)
|
||||
{
|
||||
ASSERT(NSAMPLES > 1);
|
||||
size_t* const samplesBegin = samples;
|
||||
size_t* const samplesEnd = samples + NSAMPLES;
|
||||
*samples++ = 0;
|
||||
do {
|
||||
do {
|
||||
*samples = size_t(RAND() % n);
|
||||
} while (std::find(samplesBegin, samples, *samples) != samples);
|
||||
} while (++samples < samplesEnd);
|
||||
}
|
||||
};
|
||||
|
||||
/// Pick a random sample:
|
||||
/// get a (sorted) random sample of size NSAMPLES in [0:n-1]
|
||||
/// \param NSAMPLES The number of samples to produce.
|
||||
/// \param vec_index The possible data indices.
|
||||
/// \param n The number of samples available.
|
||||
/// \param samples The random sample of NSAMPLES indices (output).
|
||||
struct UniformSamplerSorted {
|
||||
template <size_t NSAMPLES>
|
||||
inline void Sample(
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
const std::vector<size_t>& vec_index,
|
||||
#else
|
||||
size_t n,
|
||||
#endif
|
||||
size_t* samples)
|
||||
{
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
const size_t n = vec_index.size();
|
||||
#endif
|
||||
for (size_t i=0; i<NSAMPLES; ++i) {
|
||||
size_t r = (RAND()>>3)%(n-i), j;
|
||||
for (j=0; j<i && r>=samples[j]; ++j)
|
||||
++r;
|
||||
size_t j0 = j;
|
||||
for (j=i; j>j0; --j)
|
||||
samples[j] = samples[j-1];
|
||||
samples[j0] = r;
|
||||
}
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
for (size_t i=0; i<NSAMPLES; ++i)
|
||||
samples[i] = vec_index[samples[i]];
|
||||
#endif
|
||||
}
|
||||
};
|
||||
/// Same as above, but ensure that the first point is always included
|
||||
struct UniformSamplerSortedLockFirst {
|
||||
template <size_t NSAMPLES>
|
||||
inline void Sample(
|
||||
size_t n,
|
||||
size_t* samples)
|
||||
{
|
||||
ASSERT(NSAMPLES > 1);
|
||||
samples[0] = 0;
|
||||
for (size_t i=1; i<NSAMPLES; ++i) {
|
||||
size_t r = (RAND()>>3)%(n-i), j;
|
||||
for (j=0; j<i && r>=samples[j]; ++j)
|
||||
++r;
|
||||
size_t j0 = j;
|
||||
for (j=i; j>j0; --j)
|
||||
samples[j] = samples[j-1];
|
||||
samples[j0] = r;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/// Returns the requested matrix rows
|
||||
template <typename TMat, typename TRows>
|
||||
TMat ExtractRows(const TMat &A, const TRows &rows) {
|
||||
TMat compressed((int)rows.size(), A.cols);
|
||||
for (size_t i=0; i<static_cast<size_t>(rows.size()); ++i)
|
||||
A.row(rows[i]).copyTo(compressed.row((int)i));
|
||||
return compressed;
|
||||
}
|
||||
|
||||
/// ACRANSAC routine (ErrorThreshold, NFA)
|
||||
template <typename Kernel, typename Sampler>
|
||||
std::pair<double, double> ACRANSAC(
|
||||
Kernel& kernel,
|
||||
Sampler& sampler,
|
||||
std::vector<size_t>& vec_inliers,
|
||||
typename Kernel::Model& model,
|
||||
double maxThreshold = DBL_MAX,
|
||||
#ifndef ACRANSAC_SAMPLE_INLIERS
|
||||
double confidence = 0.9999,
|
||||
#endif
|
||||
size_t nIter = 0)
|
||||
{
|
||||
const size_t nData = kernel.NumSamples();
|
||||
if (nData < Kernel::MINIMUM_SAMPLES) {
|
||||
vec_inliers.clear();
|
||||
return std::make_pair(0.0,0.0);
|
||||
}
|
||||
const double maxThresholdSq = SQUARE(maxThreshold);
|
||||
std::vector<size_t> vec_sample(Kernel::MINIMUM_SAMPLES); // sample indices
|
||||
typename Kernel::Models vec_models; // up to max_models solutions
|
||||
if (nData == Kernel::MINIMUM_SAMPLES) {
|
||||
vec_inliers.resize(Kernel::MINIMUM_SAMPLES);
|
||||
std::iota(vec_inliers.begin(), vec_inliers.end(), 0);
|
||||
if (!kernel.Fit(vec_inliers, vec_models) || vec_models.size() != 1) {
|
||||
vec_inliers.clear();
|
||||
return std::make_pair(0.0,0.0);
|
||||
}
|
||||
model = vec_models[0];
|
||||
return std::make_pair(maxThresholdSq,0.0);
|
||||
}
|
||||
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
// Possible sampling indices (could change in the optimization phase)
|
||||
std::vector<size_t> vec_index(nData);
|
||||
for (size_t i = 0; i < nData; ++i)
|
||||
vec_index[i] = i;
|
||||
#endif
|
||||
|
||||
// Precompute log combi
|
||||
const double loge0 = log10((double)Kernel::MAX_MODELS * (nData-Kernel::MINIMUM_SAMPLES));
|
||||
std::vector<float> vec_logc_n, vec_logc_k;
|
||||
makelogcombi_n<float>(nData, vec_logc_n);
|
||||
makelogcombi_k<float, Kernel::MINIMUM_SAMPLES>(nData, vec_logc_k);
|
||||
|
||||
// Output parameters
|
||||
double minNFA = DBL_MAX;
|
||||
double errorSqMax = DBL_MAX;
|
||||
|
||||
// Reserve 10% of iterations for focused sampling
|
||||
if (nIter == 0)
|
||||
nIter = MINF((size_t)4000, MAXF((size_t)300, nData*3/2));
|
||||
const size_t nMinIter = nIter*8/100;
|
||||
|
||||
// Main estimation loop
|
||||
ErrorIndexArr vec_residuals(nData); // [residual,index]
|
||||
for (size_t iter=0; iter<nIter || iter<nMinIter; ++iter) {
|
||||
// Get random sample
|
||||
sampler.template Sample<Kernel::MINIMUM_SAMPLES>(
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
vec_index,
|
||||
#else
|
||||
nData,
|
||||
#endif
|
||||
&vec_sample[0]
|
||||
);
|
||||
|
||||
if (!kernel.Fit(vec_sample, vec_models))
|
||||
continue;
|
||||
|
||||
// Evaluate models
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
bool better = false;
|
||||
#else
|
||||
const size_t nTrialsRound = nIter;
|
||||
#endif
|
||||
for (typename Kernel::Models::const_iterator itModel=vec_models.cbegin(); itModel!=vec_models.cend(); ++itModel) {
|
||||
// Residuals computation and ordering
|
||||
kernel.EvaluateModel(*itModel);
|
||||
for (size_t i = 0; i < nData; ++i)
|
||||
vec_residuals[i] = ErrorIndex(kernel.Error(i), i);
|
||||
std::sort(vec_residuals.begin(), vec_residuals.end());
|
||||
|
||||
// Most meaningful discrimination inliers/outliers
|
||||
ErrorIndex best = bestNFA<Kernel::MINIMUM_SAMPLES>(
|
||||
kernel.logalpha0(),
|
||||
vec_residuals,
|
||||
loge0,
|
||||
maxThresholdSq,
|
||||
vec_logc_n,
|
||||
vec_logc_k,
|
||||
kernel.multError());
|
||||
|
||||
if (best.first < minNFA /*&& vec_residuals[best.second-1].first < errorSqMax*/) {
|
||||
// A better model was found
|
||||
minNFA = best.first;
|
||||
vec_inliers.resize(best.second);
|
||||
for (size_t i=0; i<best.second; ++i)
|
||||
vec_inliers[i] = vec_residuals[i].second;
|
||||
errorSqMax = vec_residuals[best.second-1].first; // error threshold
|
||||
model = *itModel;
|
||||
DEBUG_LEVEL(4, "\titer=% 3u inliers=%u threshold=%g nfa=%g", iter, best.second, SQRT(errorSqMax), minNFA);
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
better = true;
|
||||
#else
|
||||
nIter = cvRANSACUpdateNumIters(confidence, (double)(nData - best.second)/nData, unsigned(Kernel::MINIMUM_SAMPLES), (unsigned)nTrialsRound);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
vec_models.clear();
|
||||
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
if (better && minNFA<0 && vec_inliers.size()>nData/3) {
|
||||
// Optimization: draw samples among best set of inliers so far
|
||||
vec_index = vec_inliers;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if (minNFA >= 0)
|
||||
vec_inliers.clear();
|
||||
|
||||
return std::make_pair(errorSqMax, minNFA);
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
|
||||
|
||||
/// RANSAC routine (standard robust fitter, based on a known threshold)
|
||||
template <typename Kernel, typename Sampler>
|
||||
void RANSAC(
|
||||
Kernel& kernel,
|
||||
Sampler& sampler,
|
||||
std::vector<size_t>& vec_inliers,
|
||||
typename Kernel::Model& model,
|
||||
double threshold,
|
||||
#ifndef ACRANSAC_SAMPLE_INLIERS
|
||||
double confidence = 0.9999,
|
||||
#endif
|
||||
size_t nIter = 0)
|
||||
{
|
||||
vec_inliers.clear();
|
||||
const size_t nData = kernel.NumSamples();
|
||||
if (nData < Kernel::MINIMUM_SAMPLES)
|
||||
return;
|
||||
const double thresholdSq = SQUARE(threshold);
|
||||
std::vector<size_t> vec_sample(Kernel::MINIMUM_SAMPLES); // sample indices
|
||||
typename Kernel::Models vec_models; // up to max_models solutions
|
||||
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
// Possible sampling indices (could change in the optimization phase)
|
||||
std::vector<size_t> vec_index(nData);
|
||||
for (size_t i = 0; i < nData; ++i)
|
||||
vec_index[i] = i;
|
||||
#endif
|
||||
|
||||
// Reserve 10% of iterations for focused sampling
|
||||
if (nIter == 0)
|
||||
nIter = MINF((size_t)4000, MAXF((size_t)300, nData*3/2));
|
||||
const size_t nMinIter = nIter*8/100;
|
||||
|
||||
// Main estimation loop
|
||||
#ifdef _USE_MSAC
|
||||
double best_score = thresholdSq*nData;
|
||||
#endif
|
||||
std::vector<size_t> vec_inliers_tmp; vec_inliers_tmp.reserve(nData); vec_inliers.reserve(nData);
|
||||
for (size_t iter=0; iter<nIter || iter<nMinIter; ++iter) {
|
||||
// Get random sample
|
||||
sampler.template Sample<Kernel::MINIMUM_SAMPLES>(
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
vec_index,
|
||||
#else
|
||||
nData,
|
||||
#endif
|
||||
&vec_sample[0]
|
||||
);
|
||||
|
||||
if (!kernel.Fit(vec_sample, vec_models))
|
||||
continue;
|
||||
|
||||
// Evaluate models
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
bool better = false;
|
||||
#else
|
||||
const size_t nTrialsRound = nIter;
|
||||
#endif
|
||||
for (typename Kernel::Models::const_iterator itModel=vec_models.cbegin(); itModel!=vec_models.cend(); ++itModel) {
|
||||
// Residuals computation and ordering
|
||||
#ifdef _USE_MSAC
|
||||
double score = 0;
|
||||
#endif
|
||||
vec_inliers_tmp.clear();
|
||||
kernel.EvaluateModel(*itModel);
|
||||
for (size_t i = 0; i < nData; ++i) {
|
||||
const double errSq = kernel.Error(i);
|
||||
if (errSq <= thresholdSq) {
|
||||
#ifdef _USE_MSAC
|
||||
score += errSq;
|
||||
#endif
|
||||
vec_inliers_tmp.push_back(i);
|
||||
} else {
|
||||
#ifdef _USE_MSAC
|
||||
score += thresholdSq;
|
||||
#endif
|
||||
}
|
||||
#ifdef _USE_MSAC
|
||||
if (score >= best_score)
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
// Store best model
|
||||
#ifdef _USE_MSAC
|
||||
if (score < best_score) {
|
||||
#else
|
||||
if (vec_inliers_tmp.size() > vec_inliers.size()) {
|
||||
#endif
|
||||
// A better model was found
|
||||
model = *itModel;
|
||||
vec_inliers.swap(vec_inliers_tmp);
|
||||
if (vec_inliers.size() == nData) {
|
||||
nIter = 0; // force loop exit
|
||||
break;
|
||||
}
|
||||
#ifdef _USE_MSAC
|
||||
best_score = score;
|
||||
DEBUG_LEVEL(4, "\titer=%3u inliers=%u score=%g", iter, vec_inliers.size(), best_score);
|
||||
#else
|
||||
DEBUG_LEVEL(4, "\titer=%3u inliers=%u", iter, vec_inliers.size());
|
||||
#endif
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
better = true;
|
||||
#else
|
||||
#ifdef _USE_MSAC
|
||||
nIter = cvRANSACUpdateNumIters(confidence, (double)score/((double)nData*thresholdSq), unsigned(Kernel::MINIMUM_SAMPLES), (unsigned)nTrialsRound);
|
||||
#else
|
||||
nIter = cvRANSACUpdateNumIters(confidence, (double)(nData-vec_inliers.size())/nData, unsigned(Kernel::MINIMUM_SAMPLES), (unsigned)nTrialsRound);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
}
|
||||
vec_models.clear();
|
||||
|
||||
#ifdef ACRANSAC_SAMPLE_INLIERS
|
||||
if (better && vec_inliers.size()>nData/3) {
|
||||
// Optimization: draw samples among best set of inliers so far
|
||||
vec_index = vec_inliers;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if (vec_inliers.size() < Kernel::MINIMUM_SAMPLES)
|
||||
vec_inliers.clear();
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
|
||||
|
||||
/// Two view Kernel adaptator for the A contrario model estimator
|
||||
/// Handle data normalization and compute the corresponding logalpha 0
|
||||
/// that depends of the error model (point to line, or point to point)
|
||||
/// This kernel adaptor is working for affine, homography, fundamental matrix
|
||||
/// estimation.
|
||||
template <typename SOLVER>
|
||||
class ACKernelAdaptor
|
||||
{
|
||||
public:
|
||||
typedef SOLVER Solver;
|
||||
typedef typename SOLVER::Model Model;
|
||||
typedef typename SOLVER::Models Models;
|
||||
|
||||
enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES };
|
||||
enum { MAX_MODELS = Solver::MAX_MODELS };
|
||||
|
||||
ACKernelAdaptor(
|
||||
const DMatrix32F &x1, float w1, float h1,
|
||||
const DMatrix32F &x2, float w2, float h2,
|
||||
bool bPointToLine = true)
|
||||
: x1_(x1), x2_(x2),
|
||||
bPointToLine_(bPointToLine)
|
||||
{
|
||||
ASSERT(2 == x1_.cols);
|
||||
ASSERT(x1_.rows == x2_.rows);
|
||||
ASSERT(x1_.cols == x2_.cols);
|
||||
ASSERT(NumSamples() >= MINIMUM_SAMPLES);
|
||||
|
||||
// LogAlpha0 is used to make error data scale invariant
|
||||
const float s2 = 1.0f;
|
||||
if (bPointToLine) {
|
||||
// Ratio of containing diagonal image rectangle over image area
|
||||
const float D = sqrt(w2*w2 + h2*h2); // diameter
|
||||
const float A = w2*h2; // area
|
||||
logalpha0_ = log10(2.0f*D/A/s2);
|
||||
}
|
||||
else {
|
||||
// ratio of area : unit circle over image area
|
||||
logalpha0_ = log10(M_PI/(w2*h2)/(s2*s2));
|
||||
}
|
||||
}
|
||||
|
||||
inline bool Fit(const std::vector<size_t>& samples, Models& models) const {
|
||||
const DMatrix32F x1(ExtractRows(x1_, samples));
|
||||
if (CheckCollinearity(x1.ptr<const Point2f>(), x1.rows))
|
||||
return false;
|
||||
const DMatrix32F x2(ExtractRows(x2_, samples));
|
||||
if (CheckCollinearity(x2.ptr<const Point2f>(), x2.rows))
|
||||
return false;
|
||||
Solver::Solve(x1, x2, models);
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void EvaluateModel(const Model& model) {
|
||||
model2evaluate = model;
|
||||
}
|
||||
|
||||
inline double Error(size_t sample) const {
|
||||
return Solver::Error(model2evaluate, *x1_.ptr<const Point2f>((int)sample), *x2_.ptr<const Point2f>((int)sample));
|
||||
}
|
||||
|
||||
inline size_t NumSamples() const { return static_cast<size_t>(x1_.rows); }
|
||||
inline double logalpha0() const { return logalpha0_; }
|
||||
inline double multError() const { return (bPointToLine_ ? 0.5 : 1.0); }
|
||||
|
||||
protected:
|
||||
DMatrix32F x1_, x2_; // Normalized input data
|
||||
double logalpha0_; // Alpha0 is used to make the error adaptive to the image size
|
||||
bool bPointToLine_;// Store if error model is pointToLine or point to point
|
||||
Model model2evaluate; // current model to be evaluated
|
||||
};
|
||||
|
||||
/// Pose/Resection Kernel adaptator for the A contrario model estimator
|
||||
template <typename SOLVER>
|
||||
class ACKernelAdaptorResection
|
||||
{
|
||||
public:
|
||||
typedef SOLVER Solver;
|
||||
typedef typename SOLVER::Model Model;
|
||||
typedef typename SOLVER::Models Models;
|
||||
|
||||
enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES };
|
||||
enum { MAX_MODELS = Solver::MAX_MODELS };
|
||||
|
||||
ACKernelAdaptorResection(const DMatrix32F &x2d, const DMatrix &x3D)
|
||||
: x2d_(x2d), x3D_(x3D),
|
||||
logalpha0_(log10(M_PI))
|
||||
{
|
||||
ASSERT(2 == x2d_.cols);
|
||||
ASSERT(3 == x3D_.cols);
|
||||
ASSERT(x2d_.rows == x3D_.rows);
|
||||
ASSERT(NumSamples() >= MINIMUM_SAMPLES);
|
||||
}
|
||||
|
||||
inline bool Fit(const std::vector<size_t>& samples, Models& models) const {
|
||||
const DMatrix32F x1(ExtractRows(x2d_, samples));
|
||||
if (CheckCollinearity(x1.ptr<const Point2f>(), x1.rows))
|
||||
return false;
|
||||
const DMatrix x2(ExtractRows(x3D_, samples));
|
||||
Solver::Solve(x1, x2, models);
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void EvaluateModel(const Model &model) {
|
||||
model2evaluate = model;
|
||||
}
|
||||
|
||||
inline double Error(size_t sample) const {
|
||||
return Solver::Error(model2evaluate, *x2d_.ptr<const Point2f>((int)sample), *x3D_.ptr<const Point3>((int)sample));
|
||||
}
|
||||
|
||||
inline size_t NumSamples() const { return x2d_.rows; }
|
||||
inline double logalpha0() const { return logalpha0_; }
|
||||
inline double multError() const { return 1.0; } // point to point error
|
||||
|
||||
protected:
|
||||
DMatrix32F x2d_;
|
||||
DMatrix x3D_;
|
||||
double logalpha0_; // Alpha0 is used to make the error adaptive to the image size
|
||||
Model model2evaluate; // current model to be evaluated
|
||||
};
|
||||
|
||||
/// Essential matrix Kernel adaptator for the A contrario model estimator
|
||||
template <typename SOLVER>
|
||||
class ACKernelAdaptorEssential
|
||||
{
|
||||
public:
|
||||
typedef SOLVER Solver;
|
||||
typedef typename SOLVER::Model Model;
|
||||
typedef typename SOLVER::Models Models;
|
||||
|
||||
enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES };
|
||||
enum { MAX_MODELS = Solver::MAX_MODELS };
|
||||
|
||||
ACKernelAdaptorEssential(
|
||||
const DMatrix32F &x1, float w1, float h1, const Matrix3x3f& invK1,
|
||||
const DMatrix32F &x2, float w2, float h2, const Matrix3x3f& invK2)
|
||||
: solver(invK1, invK2),
|
||||
x1_(x1), x2_(x2)
|
||||
{
|
||||
ASSERT(2 == x1_.cols);
|
||||
ASSERT(x1_.rows == x2_.rows);
|
||||
ASSERT(x1_.cols == x2_.cols);
|
||||
ASSERT(NumSamples() >= MINIMUM_SAMPLES);
|
||||
|
||||
//Point to line probability (line is the epipolar line)
|
||||
const float D = sqrt(w2*w2 + h2*h2); // diameter
|
||||
const float A = w2*h2+1.f; // area
|
||||
logalpha0_ = log10(2.0f*D/A*0.5f);
|
||||
}
|
||||
|
||||
inline bool Fit(const std::vector<size_t>& samples, Models& models) const {
|
||||
const DMatrix32F x1(ExtractRows(x1_, samples));
|
||||
if (CheckCollinearity(x1.ptr<const Point2f>(), x1.rows))
|
||||
return false;
|
||||
const DMatrix32F x2(ExtractRows(x2_, samples));
|
||||
if (CheckCollinearity(x2.ptr<const Point2f>(), x2.rows))
|
||||
return false;
|
||||
solver.Solve(x1, x2, models);
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void EvaluateModel(const Model &model) {
|
||||
solver.EvaluateModel(model);
|
||||
}
|
||||
|
||||
inline double Error(size_t sample) const {
|
||||
return solver.Error(*x1_.ptr<const Point2f>((int)sample), *x2_.ptr<const Point2f>((int)sample));
|
||||
}
|
||||
|
||||
inline size_t NumSamples() const { return x1_.rows; }
|
||||
inline double logalpha0() const { return logalpha0_; }
|
||||
inline double multError() const { return 0.5; } // point to line error
|
||||
|
||||
protected:
|
||||
Solver solver;
|
||||
DMatrix32F x1_, x2_; // image point and camera plane point.
|
||||
double logalpha0_; // Alpha0 is used to make the error adaptive to the image size
|
||||
};
|
||||
/*----------------------------------------------------------------*/
|
||||
|
||||
} // namespace SEACAVE
|
||||
|
||||
#endif // __SEACAVE_AUTO_ESTIMATOR_H__
|
||||
Reference in new issue
Block a user