Skip to content

O
opencv
Commit adac8c04 authored by Olexa Bilaniuk's avatar Olexa Bilaniuk
Browse files
Options

Converted to C++ style, + bugfixes. 

The code has been refactored in response to feedback on Pull Request

Also, outputZeroH() now also zeroes the inlier set, much like
outputModel().
parent 87c2b819
Hide whitespace changes
Inline Side-by-side
Showing with 643 additions and 540 deletions
modules/calib3d/src/fundam.cpp
View file @ adac8c04
...@@ -303,8 +303,7 @@ static bool createAndRunRHORegistrator(double confidence, ...@@ -303,8 +303,7 @@ static bool createAndRunRHORegistrator(double confidence,
* initialized, used, then finalized. * initialized, used, then finalized.
*/ */
RHO_HEST_REFC p; RHO_HEST_REFC* p = rhoRefCInit();
rhoRefCInit(&p);
/** /**
* Optional. Ideally, the context would survive across calls to * Optional. Ideally, the context would survive across calls to
...@@ -312,7 +311,7 @@ static bool createAndRunRHORegistrator(double confidence, ...@@ -312,7 +311,7 @@ static bool createAndRunRHORegistrator(double confidence,
* to pay is marginally more computational work than strictly needed. * to pay is marginally more computational work than strictly needed.
*/ */
rhoRefCEnsureCapacity(&p, npoints, beta); rhoRefCEnsureCapacity(p, npoints, beta);
/** /**
* The critical call. All parameters are heavily documented in rhorefc.h. * The critical call. All parameters are heavily documented in rhorefc.h.
...@@ -325,7 +324,7 @@ static bool createAndRunRHORegistrator(double confidence, ...@@ -325,7 +324,7 @@ static bool createAndRunRHORegistrator(double confidence,
* this behaviour is too problematic. * this behaviour is too problematic.
*/ */
result = !!rhoRefC(&p, result = !!rhoRefC(p,
(const float*)src.data, (const float*)src.data,
(const float*)dst.data, (const float*)dst.data,
(char*) tempMask.data, (char*) tempMask.data,
...@@ -344,7 +343,7 @@ static bool createAndRunRHORegistrator(double confidence, ...@@ -344,7 +343,7 @@ static bool createAndRunRHORegistrator(double confidence,
* Cleanup. * Cleanup.
*/ */
rhoRefCFini(&p); rhoRefCFini(p);
/* Convert float homography to double precision. */ /* Convert float homography to double precision. */
tmpH.convertTo(_H, CV_64FC1); tmpH.convertTo(_H, CV_64FC1);
......
modules/calib3d/src/rhorefc.cpp
View file @ adac8c04
...@@ -44,6 +44,7 @@ ...@@ -44,6 +44,7 @@
*/ */
/* Includes */ /* Includes */
#include <precomp.hpp>
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h> #include <stdio.h>
#include <stdint.h> #include <stdint.h>
...@@ -52,88 +53,203 @@ ...@@ -52,88 +53,203 @@
#include <limits.h> #include <limits.h>
#include <float.h> #include <float.h>
#include <math.h> #include <math.h>
#include <vector>
#include "rhorefc.h" #include "rhorefc.h"
/* Defines */ /* Defines */
#define MEM_ALIGN 32 const int MEM_ALIGN = 32;
#define HSIZE (3*3*sizeof(float)) const size_t HSIZE = (3*3*sizeof(float));
#define MIN_DELTA_CHNG 0.1 const double MIN_DELTA_CHNG = 0.1;
#define REL_CHNG(a, b) (fabs((a) - (b))/(a)) const double CHI_STAT = 2.706;
#define CHNG_SIGNIFICANT(a, b) (REL_CHNG(a, b) > MIN_DELTA_CHNG) const double CHI_SQ = 1.645;
#define CHI_STAT 2.706 const double RLO = 0.25;
#define CHI_SQ 1.645 const double RHI = 0.75;
#define RLO 0.25 const int MAXLEVMARQITERS = 100;
#define RHI 0.75 const int SMPL_SIZE = 4; /* 4 points required per model */
#define MAXLEVMARQITERS 100 const int SPRT_T_M = 25; /* Guessing 25 match evlauations / 1 model generation */
#define m 4 /* 4 points required per model */ const int SPRT_M_S = 1; /* 1 model per sample */
#define SPRT_T_M 25 /* Guessing 25 match evlauations / 1 model generation */ const double SPRT_EPSILON = 0.1; /* No explanation */
#define SPRT_M_S 1 /* 1 model per sample */ const double SPRT_DELTA = 0.01; /* No explanation */
#define SPRT_EPSILON 0.1 /* No explanation */ const double LM_GAIN_LO = 0.25; /* See sacLMGain(). */
#define SPRT_DELTA 0.01 /* No explanation */ const double LM_GAIN_HI = 0.75; /* See sacLMGain(). */
#define LM_GAIN_LO 0.25 /* See sacLMGain(). */
#define LM_GAIN_HI 0.75 /* See sacLMGain(). */
/* For the sake of cv:: namespace ONLY: */ /* For the sake of cv:: namespace ONLY: */
#ifdef __cplusplus
namespace cv{/* For C support, replace with extern "C" { */ namespace cv{/* For C support, replace with extern "C" { */
#endif
/* Data Structures */ /* Data Structures */
struct RHO_HEST_REFC{
/**
* Virtual Arguments.
*
* Exactly the same as at function call, except:
* - minInl is enforced to be >= 4.
*/
struct{
const float* src;
const float* dst;
char* inl;
unsigned N;
float maxD;
unsigned maxI;
unsigned rConvg;
double cfd;
unsigned minInl;
double beta;
unsigned flags;
const float* guessH;
float* finalH;
} arg;
/* PROSAC Control */
struct{
unsigned i; /* Iteration Number */
unsigned phNum; /* Phase Number */
unsigned phEndI; /* Phase End Iteration */
double phEndFpI; /* Phase floating-point End Iteration */
unsigned phMax; /* Termination phase number */
unsigned phNumInl; /* Number of inliers for termination phase */
unsigned numModels; /* Number of models tested */
unsigned* smpl; /* Sample of match indexes */
} ctrl;
/* Current model being tested */
struct{
float* pkdPts; /* Packed points */
float* H; /* Homography */
char* inl; /* Mask of inliers */
unsigned numInl; /* Number of inliers */
} curr;
/* Best model (so far) */
struct{
float* H; /* Homography */
char* inl; /* Mask of inliers */
unsigned numInl; /* Number of inliers */
} best;
/* Non-randomness criterion */
struct{
std::vector<unsigned> tbl; /* Non-Randomness: Table */
unsigned size; /* Non-Randomness: Size */
double beta; /* Non-Randomness: Beta */
} nr;
/* SPRT Evaluator */
struct{
double t_M; /* t_M */
double m_S; /* m_S */
double epsilon; /* Epsilon */
double delta; /* delta */
double A; /* SPRT Threshold */
unsigned Ntested; /* Number of points tested */
unsigned Ntestedtotal; /* Number of points tested in total */
int good; /* Good/bad flag */
double lambdaAccept; /* Accept multiplier */
double lambdaReject; /* Reject multiplier */
} eval;
/* Levenberg-Marquardt Refinement */
struct{
float* ws; /* Levenberg-Marqhard Workspace */
float (* JtJ)[8]; /* JtJ matrix */
float (* tmp1)[8]; /* Temporary 1 */
float* Jte; /* Jte vector */
} lm;
/* Initialized? */
int init;
/* Empty constructors and destructors */
public:
RHO_HEST_REFC();
private: /* Forbid copying. */
RHO_HEST_REFC(const RHO_HEST_REFC&);
public:
~RHO_HEST_REFC();
/* Methods to implement external interface */
inline int initialize(void);
inline int sacEnsureCapacity(unsigned N, double beta);
inline void finalize(void);
unsigned rhoRefC(const float* src, /* Source points */
const float* dst, /* Destination points */
char* inl, /* Inlier mask */
unsigned N, /* = src.length = dst.length = inl.length */
float maxD, /* Works: 3.0 */
unsigned maxI, /* Works: 2000 */
unsigned rConvg, /* Works: 2000 */
double cfd, /* Works: 0.995 */
unsigned minInl, /* Minimum: 4 */
double beta, /* Works: 0.35 */
unsigned flags, /* Works: 0 */
const float* guessH, /* Extrinsic guess, NULL if none provided */
float* finalH); /* Final result. */
/* Methods to implement internals */
inline int initRun(void);
inline void finiRun(void);
inline int haveExtrinsicGuess(void);
inline int hypothesize(void);
inline int verify(void);
inline int isNREnabled(void);
inline int isRefineEnabled(void);
inline int isFinalRefineEnabled(void);
inline int PROSACPhaseEndReached(void);
inline void PROSACGoToNextPhase(void);
inline void getPROSACSample(void);
inline int isSampleDegenerate(void);
inline void generateModel(void);
inline int isModelDegenerate(void);
inline void evaluateModelSPRT(void);
inline void updateSPRT(void);
inline void designSPRTTest(void);
inline int isBestModel(void);
inline int isBestModelGoodEnough(void);
inline void saveBestModel(void);
inline void nStarOptimize(void);
inline void updateBounds(void);
inline void outputModel(void);
inline void outputZeroH(void);
inline int canRefine(void);
inline void refine(void);
};
/* Prototypes */
static inline void* almalloc(size_t nBytes);
static inline void alfree(void* ptr);
static inline int sacInitRun(RHO_HEST_REFC* p);
static inline void sacFiniRun(RHO_HEST_REFC* p);
static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p);
static inline int sacHypothesize(RHO_HEST_REFC* p);
static inline int sacVerify(RHO_HEST_REFC* p);
static inline int sacIsNREnabled(RHO_HEST_REFC* p);
static inline int sacIsRefineEnabled(RHO_HEST_REFC* p);
static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p);
static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p);
static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p);
static inline void sacGetPROSACSample(RHO_HEST_REFC* p);
static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p);
static inline void sacGenerateModel(RHO_HEST_REFC* p);
static inline int sacIsModelDegenerate(RHO_HEST_REFC* p);
static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p);
static inline void sacUpdateSPRT(RHO_HEST_REFC* p);
static inline void sacDesignSPRTTest(RHO_HEST_REFC* p);
static inline int sacIsBestModel(RHO_HEST_REFC* p);
static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p);
static inline void sacSaveBestModel(RHO_HEST_REFC* p);
static inline void sacInitNonRand(double beta,
unsigned start,
unsigned N,
unsigned* nonRandMinInl);
static inline void sacNStarOptimize(RHO_HEST_REFC* p);
static inline void sacUpdateBounds(RHO_HEST_REFC* p);
static inline void sacOutputModel(RHO_HEST_REFC* p);
static inline void sacOutputZeroH(RHO_HEST_REFC* p);
static inline double sacInitPEndFpI(const unsigned ransacConvg,
const unsigned n, /**
const unsigned s); * Prototypes for purely-computational code.
static inline void sacRndSmpl(unsigned sampleSize, */
unsigned* currentSample,
unsigned dataSetSize); static inline void* almalloc(size_t nBytes);
static inline double sacRandom(void); static inline void alfree (void* ptr);
static inline unsigned sacCalcIterBound(double confidence,
double inlierRate, static inline void sacInitNonRand (double beta,
unsigned sampleSize, unsigned start,
unsigned maxIterBound); unsigned N,
static inline void hFuncRefC(float* packedPoints, float* H); unsigned* nonRandMinInl);
static inline int sacCanRefine(RHO_HEST_REFC* p); static inline double sacInitPEndFpI (const unsigned ransacConvg,
static inline void sacRefine(RHO_HEST_REFC* p); const unsigned n,
const unsigned s);
static inline void sacRndSmpl (unsigned sampleSize,
unsigned* currentSample,
unsigned dataSetSize);
static inline double sacRandom (void);
static inline unsigned sacCalcIterBound (double confidence,
double inlierRate,
unsigned sampleSize,
unsigned maxIterBound);
static inline void hFuncRefC (float* packedPoints, float* H);
static inline void sacCalcJacobianErrors(const float* restrict H, static inline void sacCalcJacobianErrors(const float* restrict H,
const float* restrict src, const float* restrict src,
const float* restrict dst, const float* restrict dst,
...@@ -142,25 +258,175 @@ static inline void sacCalcJacobianErrors(const float* restrict H, ...@@ -142,25 +258,175 @@ static inline void sacCalcJacobianErrors(const float* restrict H,
float (* restrict JtJ)[8], float (* restrict JtJ)[8],
float* restrict Jte, float* restrict Jte,
float* restrict Sp); float* restrict Sp);
static inline float sacLMGain(const float* dH, static inline float sacLMGain (const float* dH,
const float* Jte, const float* Jte,
const float S, const float S,
const float newS, const float newS,
const float lambda); const float lambda);
static inline int sacChol8x8Damped (const float (*A)[8], static inline int sacChol8x8Damped (const float (*A)[8],
float lambda, float lambda,
float (*L)[8]); float (*L)[8]);
static inline void sacTRInv8x8(const float (*L)[8], static inline void sacTRInv8x8 (const float (*L)[8],
float (*M)[8]); float (*M)[8]);
static inline void sacTRISolve8x8(const float (*L)[8], static inline void sacTRISolve8x8 (const float (*L)[8],
const float* Jte, const float* Jte,
float* dH); float* dH);
static inline void sacSub8x1(float* Hout, const float* H, const float* dH); static inline void sacSub8x1 (float* Hout,
const float* H,
const float* dH);
/* Functions */ /* Functions */
/**
* External access to context constructor.
*
* @return A pointer to the context if successful; NULL if an error occured.
*/
RHO_HEST_REFC* rhoRefCInit(void){
RHO_HEST_REFC* p = new RHO_HEST_REFC;
if(!p->initialize()){
delete p;
p = NULL;
}
return p;
}
/**
* External access to non-randomness table resize.
*/
int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta){
return p->sacEnsureCapacity(N, beta);
}
/**
* External access to context destructor.
*
* @param [in] p The initialized estimator context to finalize.
*/
void rhoRefCFini(RHO_HEST_REFC* p){
delete p;
}
/**
* Estimates the homography using the given context, matches and parameters to
* PROSAC.
*
* @param [in/out] p The context to use for homography estimation. Must
* be already initialized. Cannot be NULL.
* @param [in] src The pointer to the source points of the matches.
* Must be aligned to 4 bytes. Cannot be NULL.
* @param [in] dst The pointer to the destination points of the matches.
* Must be aligned to 16 bytes. Cannot be NULL.
* @param [out] inl The pointer to the output mask of inlier matches.
* Must be aligned to 16 bytes. May be NULL.
* @param [in] N The number of matches.
* @param [in] maxD The maximum distance.
* @param [in] maxI The maximum number of PROSAC iterations.
* @param [in] rConvg The RANSAC convergence parameter.
* @param [in] cfd The required confidence in the solution.
* @param [in] minInl The minimum required number of inliers.
* @param [in] beta The beta-parameter for the non-randomness criterion.
* @param [in] flags A union of flags to control the estimation.
* @param [in] guessH An extrinsic guess at the solution H, or NULL if
* none provided.
* @param [out] finalH The final estimation of H, or the zero matrix if
* the minimum number of inliers was not met.
* Cannot be NULL.
* @return The number of inliers if the minimum number of
* inliers for acceptance was reached; 0 otherwise.
*/
unsigned rhoRefC(RHO_HEST_REFC* p, /* Homography estimation context. */
const float* src, /* Source points */
const float* dst, /* Destination points */
char* inl, /* Inlier mask */
unsigned N, /* = src.length = dst.length = inl.length */
float maxD, /* Works: 3.0 */
unsigned maxI, /* Works: 2000 */
unsigned rConvg, /* Works: 2000 */
double cfd, /* Works: 0.995 */
unsigned minInl, /* Minimum: 4 */
double beta, /* Works: 0.35 */
unsigned flags, /* Works: 0 */
const float* guessH, /* Extrinsic guess, NULL if none provided */
float* finalH){ /* Final result. */
return p->rhoRefC(src, dst, inl, N, maxD, maxI, rConvg, cfd, minInl, beta,
flags, guessH, finalH);
}
/**
* Allocate memory aligned to a boundary of MEMALIGN.
*/
static inline void* almalloc(size_t nBytes){
if(nBytes){
unsigned char* ptr = (unsigned char*)malloc(MEM_ALIGN + nBytes);
if(ptr){
unsigned char* adj = (unsigned char*)(((intptr_t)(ptr+MEM_ALIGN))&((intptr_t)(-MEM_ALIGN)));
ptrdiff_t diff = adj - ptr;
adj[-1] = (unsigned char)(diff - 1);
return adj;
}
}
return NULL;
}
/**
* Free aligned memory.
*
* If argument is NULL, do nothing in accordance with free() semantics.
*/
static inline void alfree(void* ptr){
if(ptr){
unsigned char* cptr = (unsigned char*)ptr;
free(cptr - (ptrdiff_t)cptr[-1] - 1);
}
}
/**
* Constructor for RHO_HEST_REFC.
*
* Does nothing. True initialization is done by initialize().
*/
RHO_HEST_REFC::RHO_HEST_REFC() : init(0){
}
/**
* Private copy constructor for RHO_HEST_REFC. Disabled.
*/
RHO_HEST_REFC::RHO_HEST_REFC(const RHO_HEST_REFC&) : init(0){
}
/**
* Destructor for RHO_HEST_REFC.
*/
RHO_HEST_REFC::~RHO_HEST_REFC(){
if(init){
finalize();
}
}
/** /**
* Initialize the estimator context, by allocating the aligned buffers * Initialize the estimator context, by allocating the aligned buffers
* internally needed. * internally needed.
...@@ -172,46 +438,63 @@ static inline void sacSub8x1(float* Hout, const float* H, const float* dH); ...@@ -172,46 +438,63 @@ static inline void sacSub8x1(float* Hout, const float* H, const float* dH);
* - The buffer for the best-so-far homography * - The buffer for the best-so-far homography
* - Optionally, the non-randomness criterion table * - Optionally, the non-randomness criterion table
* *
* @param [in/out] p The uninitialized estimator context to initialize. * Returns 0 if unsuccessful and non-0 otherwise.
* @return 0 if successful; non-zero if an error occured.
*/ */
int rhoRefCInit(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::initialize(void){
memset(p, 0, sizeof(*p)); ctrl.smpl = (unsigned*)almalloc(SMPL_SIZE*sizeof(*ctrl.smpl));
p->ctrl.smpl = (unsigned*)almalloc(m*sizeof(*p->ctrl.smpl));
p->curr.pkdPts = (float*) almalloc(m*2*2*sizeof(*p->curr.pkdPts)); curr.pkdPts = (float*) almalloc(SMPL_SIZE*2*2*sizeof(*curr.pkdPts));
p->curr.H = (float*) almalloc(HSIZE); curr.H = (float*) almalloc(HSIZE);
p->curr.inl = NULL; curr.inl = NULL;
p->curr.numInl = 0; curr.numInl = 0;
p->best.H = (float*) almalloc(HSIZE); best.H = (float*) almalloc(HSIZE);
p->best.inl = NULL; best.inl = NULL;
p->best.numInl = 0; best.numInl = 0;
p->nr.tbl = NULL;/* By default this table is not computed. */ nr.size = 0;
p->nr.size = 0; nr.beta = 0.0;
p->nr.beta = 0.0;
p->lm.ws = NULL; lm.ws = NULL;
p->lm.JtJ = NULL; lm.JtJ = NULL;
p->lm.tmp1 = NULL; lm.tmp1 = NULL;
p->lm.Jte = NULL; lm.Jte = NULL;
int areAllAllocsSuccessful = p->ctrl.smpl && int areAllAllocsSuccessful = ctrl.smpl &&
p->curr.H && curr.H &&
p->best.H && best.H &&
p->curr.pkdPts; curr.pkdPts;
if(!areAllAllocsSuccessful){ if(!areAllAllocsSuccessful){
rhoRefCFini(p); finalize();
}else{
init = 1;
} }
return areAllAllocsSuccessful; return areAllAllocsSuccessful;
} }
/**
* Finalize.
*
* Finalize the estimator context, by freeing the aligned buffers used
* internally.
*/
inline void RHO_HEST_REFC::finalize(void){
if(init){
alfree(ctrl.smpl);
alfree(curr.H);
alfree(best.H);
alfree(curr.pkdPts);
memset(this, 0, sizeof(*this));
init = 0;
}
}
/** /**
* Ensure that the estimator context's internal table for non-randomness * Ensure that the estimator context's internal table for non-randomness
...@@ -220,7 +503,6 @@ int rhoRefCInit(RHO_HEST_REFC* p){ ...@@ -220,7 +503,6 @@ int rhoRefCInit(RHO_HEST_REFC* p){
* *
* A value of N of 0 requests deallocation of the table. * A value of N of 0 requests deallocation of the table.
* *
* @param [in] p The initialized estimator context
* @param [in] N If 0, deallocate internal table. If > 0, ensure that the * @param [in] N If 0, deallocate internal table. If > 0, ensure that the
* internal table is of at least this size, reallocating if * internal table is of at least this size, reallocating if
* necessary. * necessary.
...@@ -231,84 +513,34 @@ int rhoRefCInit(RHO_HEST_REFC* p){ ...@@ -231,84 +513,34 @@ int rhoRefCInit(RHO_HEST_REFC* p){
* Writes: nr.* * Writes: nr.*
*/ */
int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta){ inline int RHO_HEST_REFC::sacEnsureCapacity(unsigned N, double beta){
unsigned* tmp;
if(N == 0){ if(N == 0){
/* Deallocate table */ /* Clear. */
alfree(p->nr.tbl); nr.tbl.clear();
p->nr.tbl = NULL; nr.size = 0;
p->nr.size = 0; }else if(nr.beta != beta){
/* Beta changed. Redo all the work. */
nr.tbl.resize(N);
nr.beta = beta;
sacInitNonRand(nr.beta, 0, N, &nr.tbl[0]);
nr.size = N;
}else if(N > nr.size){
/* Work is partially done. Do rest of it. */
nr.tbl.resize(N);
sacInitNonRand(nr.beta, nr.size, N, &nr.tbl[nr.size]);
nr.size = N;
}else{ }else{
/* Ensure table at least as big as N and made for correct beta. */ /* Work is already done. Do nothing. */
if(p->nr.tbl && p->nr.beta == beta && p->nr.size >= N){
/* Table already correctly set up */
}else{
if(p->nr.size < N){
/* Reallocate table because it is too small. */
tmp = (unsigned*)almalloc(N*sizeof(unsigned));
if(!tmp){
return 0;
}
/* Must recalculate in whole or part. */
if(p->nr.beta != beta){
/* Beta changed; recalculate in whole. */
sacInitNonRand(beta, 0, N, tmp);
alfree(p->nr.tbl);
}else{
/* Beta did not change; Copy over any work already done. */
memcpy(tmp, p->nr.tbl, p->nr.size*sizeof(unsigned));
sacInitNonRand(beta, p->nr.size, N, tmp);
alfree(p->nr.tbl);
}
p->nr.tbl = tmp;
p->nr.size = N;
p->nr.beta = beta;
}else{
/* Might recalculate in whole, or not at all. */
if(p->nr.beta != beta){
/* Beta changed; recalculate in whole. */
sacInitNonRand(beta, 0, p->nr.size, p->nr.tbl);
p->nr.beta = beta;
}else{
/* Beta did not change; Table was already big enough. Do nothing. */
/* Besides, this is unreachable. */
}
}
}
} }
return 1; return 1;
} }
/**
* Finalize the estimator context, by freeing the aligned buffers used
* internally.
*
* @param [in] p The initialized estimator context to finalize.
*/
void rhoRefCFini(RHO_HEST_REFC* p){
alfree(p->ctrl.smpl);
alfree(p->curr.H);
alfree(p->best.H);
alfree(p->curr.pkdPts);
alfree(p->nr.tbl);
memset(p, 0, sizeof(*p));
}
/** /**
* Estimates the homography using the given context, matches and parameters to * Estimates the homography using the given context, matches and parameters to
* PROSAC. * PROSAC.
* *
* @param [in/out] p The context to use for homography estimation. Must
* be already initialized. Cannot be NULL.
* @param [in] src The pointer to the source points of the matches. * @param [in] src The pointer to the source points of the matches.
* Must be aligned to 4 bytes. Cannot be NULL. * Must be aligned to 4 bytes. Cannot be NULL.
* @param [in] dst The pointer to the destination points of the matches. * @param [in] dst The pointer to the destination points of the matches.
...@@ -332,41 +564,40 @@ void rhoRefCFini(RHO_HEST_REFC* p){ ...@@ -332,41 +564,40 @@ void rhoRefCFini(RHO_HEST_REFC* p){
* inliers for acceptance was reached; 0 otherwise. * inliers for acceptance was reached; 0 otherwise.
*/ */
unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation context. */ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */
const float* restrict src, /* Source points */ const float* dst, /* Destination points */
const float* restrict dst, /* Destination points */ char* inl, /* Inlier mask */
char* restrict inl, /* Inlier mask */ unsigned N, /* = src.length = dst.length = inl.length */
unsigned N, /* = src.length = dst.length = inl.length */ float maxD, /* Works: 3.0 */
float maxD, /* Works: 3.0 */ unsigned maxI, /* Works: 2000 */
unsigned maxI, /* Works: 2000 */ unsigned rConvg, /* Works: 2000 */
unsigned rConvg, /* Works: 2000 */ double cfd, /* Works: 0.995 */
double cfd, /* Works: 0.995 */ unsigned minInl, /* Minimum: 4 */
unsigned minInl, /* Minimum: 4 */ double beta, /* Works: 0.35 */
double beta, /* Works: 0.35 */ unsigned flags, /* Works: 0 */
unsigned flags, /* Works: 0 */ const float* guessH, /* Extrinsic guess, NULL if none provided */
const float* guessH, /* Extrinsic guess, NULL if none provided */ float* finalH){ /* Final result. */
float* finalH){ /* Final result. */
/** /**
* Setup * Setup
*/ */
p->arg.src = src; arg.src = src;
p->arg.dst = dst; arg.dst = dst;
p->arg.inl = inl; arg.inl = inl;
p->arg.N = N; arg.N = N;
p->arg.maxD = maxD; arg.maxD = maxD;
p->arg.maxI = maxI; arg.maxI = maxI;
p->arg.rConvg = rConvg; arg.rConvg = rConvg;
p->arg.cfd = cfd; arg.cfd = cfd;
p->arg.minInl = minInl; arg.minInl = minInl;
p->arg.beta = beta; arg.beta = beta;
p->arg.flags = flags; arg.flags = flags;
p->arg.guessH = guessH; arg.guessH = guessH;
p->arg.finalH = finalH; arg.finalH = finalH;
if(!sacInitRun(p)){ if(!initRun()){
sacOutputZeroH(p); outputZeroH();
sacFiniRun(p); finiRun();
return 0; return 0;
} }
...@@ -374,8 +605,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte ...@@ -374,8 +605,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte
* Extrinsic Guess * Extrinsic Guess
*/ */
if(sacHaveExtrinsicGuess(p)){ if(haveExtrinsicGuess()){
sacVerify(p); verify();
} }
...@@ -383,8 +614,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte ...@@ -383,8 +614,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte
* PROSAC Loop * PROSAC Loop
*/ */
for(p->ctrl.i=0; p->ctrl.i < p->arg.maxI; p->ctrl.i++){ for(ctrl.i=0; ctrl.i < arg.maxI; ctrl.i++){
sacHypothesize(p) && sacVerify(p); hypothesize() && verify();
} }
...@@ -392,45 +623,13 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte ...@@ -392,45 +623,13 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte
* Teardown * Teardown
*/ */
if(sacIsFinalRefineEnabled(p) && sacCanRefine(p)){ if(isFinalRefineEnabled() && canRefine()){
sacRefine(p); refine();
}
sacOutputModel(p);
sacFiniRun(p);
return sacIsBestModelGoodEnough(p) ? p->best.numInl : 0;
}
/**
* Allocate memory aligned to a boundary of MEMALIGN.
*/
static inline void* almalloc(size_t nBytes){
if(nBytes){
unsigned char* ptr = (unsigned char*)malloc(MEM_ALIGN + nBytes);
if(ptr){
unsigned char* adj = (unsigned char*)(((intptr_t)(ptr+MEM_ALIGN))&((intptr_t)(-MEM_ALIGN)));
ptrdiff_t diff = adj - ptr;
adj[-1] = (unsigned char)(diff - 1);
return adj;
}
} }
return NULL; outputModel();
} finiRun();
return isBestModelGoodEnough() ? best.numInl : 0;
/**
* Free aligned memory.
*
* If argument is NULL, do nothing in accordance with free() semantics.
*/
static inline void alfree(void* ptr){
if(ptr){
unsigned char* cptr = (unsigned char*)ptr;
free(cptr - (ptrdiff_t)cptr[-1] - 1);
}
} }
...@@ -446,7 +645,7 @@ static inline void alfree(void* ptr){ ...@@ -446,7 +645,7 @@ static inline void alfree(void* ptr){
* Writes: curr.*, best.*, ctrl.*, eval.* * Writes: curr.*, best.*, ctrl.*, eval.*
*/ */
static inline int sacInitRun(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::initRun(void){
/** /**
* Sanitize arguments. * Sanitize arguments.
* *
...@@ -454,29 +653,29 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ ...@@ -454,29 +653,29 @@ static inline int sacInitRun(RHO_HEST_REFC* p){
* mean something or other. * mean something or other.
*/ */
if(!p->arg.src || !p->arg.dst){ if(!arg.src || !arg.dst){
/* Arguments src or dst are insane, must be != NULL */ /* Arguments src or dst are insane, must be != NULL */
return 0; return 0;
} }
if(p->arg.N < m){ if(arg.N < SMPL_SIZE){
/* Argument N is insane, must be >= 4. */ /* Argument N is insane, must be >= 4. */
return 0; return 0;
} }
if(p->arg.maxD < 0){ if(arg.maxD < 0){
/* Argument maxD is insane, must be >= 0. */ /* Argument maxD is insane, must be >= 0. */
return 0; return 0;
} }
if(p->arg.cfd < 0 || p->arg.cfd > 1){ if(arg.cfd < 0 || arg.cfd > 1){
/* Argument cfd is insane, must be in [0, 1]. */ /* Argument cfd is insane, must be in [0, 1]. */
return 0; return 0;
} }
/* Clamp minInl to 4 or higher. */ /* Clamp minInl to 4 or higher. */
p->arg.minInl = p->arg.minInl < m ? m : p->arg.minInl; arg.minInl = arg.minInl < SMPL_SIZE ? SMPL_SIZE : arg.minInl;
if(sacIsNREnabled(p) && (p->arg.beta <= 0 || p->arg.beta >= 1)){ if(isNREnabled() && (arg.beta <= 0 || arg.beta >= 1)){
/* Argument beta is insane, must be in (0, 1). */ /* Argument beta is insane, must be in (0, 1). */
return 0; return 0;
} }
if(!p->arg.finalH){ if(!arg.finalH){
/* Argument finalH is insane, must be != NULL */ /* Argument finalH is insane, must be != NULL */
return 0; return 0;
} }
...@@ -491,7 +690,7 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ ...@@ -491,7 +690,7 @@ static inline int sacInitRun(RHO_HEST_REFC* p){
* substruct and the sanity-checked N and beta arguments from above. * substruct and the sanity-checked N and beta arguments from above.
*/ */
if(sacIsNREnabled(p) && !rhoRefCEnsureCapacity(p, p->arg.N, p->arg.beta)){ if(isNREnabled() && !sacEnsureCapacity(arg.N, arg.beta)){
return 0; return 0;
} }
...@@ -505,15 +704,15 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ ...@@ -505,15 +704,15 @@ static inline int sacInitRun(RHO_HEST_REFC* p){
* not, allocate one anyways internally. * not, allocate one anyways internally.
*/ */
p->best.inl = p->arg.inl ? p->arg.inl : (char*)almalloc(p->arg.N); best.inl = arg.inl ? arg.inl : (char*)almalloc(arg.N);
p->curr.inl = (char*)almalloc(p->arg.N); curr.inl = (char*)almalloc(arg.N);
if(!p->curr.inl || !p->best.inl){ if(!curr.inl || !best.inl){
return 0; return 0;
} }
memset(p->best.inl, 0, p->arg.N); memset(best.inl, 0, arg.N);
memset(p->curr.inl, 0, p->arg.N); memset(curr.inl, 0, arg.N);
/** /**
* LevMarq workspace alloc. * LevMarq workspace alloc.
...@@ -522,17 +721,17 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ ...@@ -522,17 +721,17 @@ static inline int sacInitRun(RHO_HEST_REFC* p){
* we wish to quit before doing any serious work. * we wish to quit before doing any serious work.
*/ */
if(sacIsRefineEnabled(p) || sacIsFinalRefineEnabled(p)){ if(isRefineEnabled() || isFinalRefineEnabled()){
p->lm.ws = (float*)almalloc(2*8*8*sizeof(float) + 1*8*sizeof(float)); lm.ws = (float*)almalloc(2*8*8*sizeof(float) + 1*8*sizeof(float));
if(!p->lm.ws){ if(!lm.ws){
return 0; return 0;
} }
p->lm.JtJ = (float(*)[8])(p->lm.ws + 0*8*8); lm.JtJ = (float(*)[8])(lm.ws + 0*8*8);
p->lm.tmp1 = (float(*)[8])(p->lm.ws + 1*8*8); lm.tmp1 = (float(*)[8])(lm.ws + 1*8*8);
p->lm.Jte = (float*) (p->lm.ws + 2*8*8); lm.Jte = (float*) (lm.ws + 2*8*8);
}else{ }else{
p->lm.ws = NULL; lm.ws = NULL;
} }
/** /**
...@@ -542,32 +741,32 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ ...@@ -542,32 +741,32 @@ static inline int sacInitRun(RHO_HEST_REFC* p){
* number of fields if something in the above junk failed. * number of fields if something in the above junk failed.
*/ */
p->ctrl.i = 0; ctrl.i = 0;
p->ctrl.phNum = m; ctrl.phNum = SMPL_SIZE;
p->ctrl.phEndI = 1; ctrl.phEndI = 1;
p->ctrl.phEndFpI = sacInitPEndFpI(p->arg.rConvg, p->arg.N, m); ctrl.phEndFpI = sacInitPEndFpI(arg.rConvg, arg.N, SMPL_SIZE);
p->ctrl.phMax = p->arg.N; ctrl.phMax = arg.N;
p->ctrl.phNumInl = 0; ctrl.phNumInl = 0;
p->ctrl.numModels = 0; ctrl.numModels = 0;
if(sacHaveExtrinsicGuess(p)){ if(haveExtrinsicGuess()){
memcpy(p->curr.H, p->arg.guessH, HSIZE); memcpy(curr.H, arg.guessH, HSIZE);
}else{ }else{
memset(p->curr.H, 0, HSIZE); memset(curr.H, 0, HSIZE);
} }
p->curr.numInl = 0; curr.numInl = 0;
memset(p->best.H, 0, HSIZE); memset(best.H, 0, HSIZE);
p->best.numInl = 0; best.numInl = 0;
p->eval.Ntested = 0; eval.Ntested = 0;
p->eval.Ntestedtotal = 0; eval.Ntestedtotal = 0;
p->eval.good = 1; eval.good = 1;
p->eval.t_M = SPRT_T_M; eval.t_M = SPRT_T_M;
p->eval.m_S = SPRT_M_S; eval.m_S = SPRT_M_S;
p->eval.epsilon = SPRT_EPSILON; eval.epsilon = SPRT_EPSILON;
p->eval.delta = SPRT_DELTA; eval.delta = SPRT_DELTA;
sacDesignSPRTTest(p); designSPRTTest();
return 1; return 1;
} }
...@@ -583,33 +782,33 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ ...@@ -583,33 +782,33 @@ static inline int sacInitRun(RHO_HEST_REFC* p){
* Writes: curr.inl, best.inl * Writes: curr.inl, best.inl
*/ */
static inline void sacFiniRun(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::finiRun(void){
/** /**
* If no output inlier mask was required, free both (internal) masks. * If no output inlier mask was required, free both (internal) masks.
* Else if an (external) mask was provided as argument, find the other * Else if an (external) mask was provided as argument, find the other
* (the internal one) and free it. * (the internal one) and free it.
*/ */
if(p->arg.inl){ if(arg.inl){
if(p->arg.inl == p->best.inl){ if(arg.inl == best.inl){
alfree(p->curr.inl); alfree(curr.inl);
}else{ }else{
alfree(p->best.inl); alfree(best.inl);
} }
}else{ }else{
alfree(p->best.inl); alfree(best.inl);
alfree(p->curr.inl); alfree(curr.inl);
} }
p->best.inl = NULL; best.inl = NULL;
p->curr.inl = NULL; curr.inl = NULL;
/** /**
* ₣ree the Levenberg-Marquardt workspace. * ₣ree the Levenberg-Marquardt workspace.
*/ */
alfree(p->lm.ws); alfree(lm.ws);
p->lm.ws = NULL; lm.ws = NULL;
} }
/** /**
...@@ -622,18 +821,18 @@ static inline void sacFiniRun(RHO_HEST_REFC* p){ ...@@ -622,18 +821,18 @@ static inline void sacFiniRun(RHO_HEST_REFC* p){
* non-zero otherwise. * non-zero otherwise.
*/ */
static inline int sacHypothesize(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::hypothesize(void){
if(sacPROSACPhaseEndReached(p)){ if(PROSACPhaseEndReached()){
sacPROSACGoToNextPhase(p); PROSACGoToNextPhase();
} }
sacGetPROSACSample(p); getPROSACSample();
if(sacIsSampleDegenerate(p)){ if(isSampleDegenerate()){
return 0; return 0;
} }
sacGenerateModel(p); generateModel();
if(sacIsModelDegenerate(p)){ if(isModelDegenerate()){
return 0; return 0;
} }
...@@ -649,21 +848,21 @@ static inline int sacHypothesize(RHO_HEST_REFC* p){ ...@@ -649,21 +848,21 @@ static inline int sacHypothesize(RHO_HEST_REFC* p){
* Returns 1. * Returns 1.
*/ */
static inline int sacVerify(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::verify(void){
sacEvaluateModelSPRT(p); evaluateModelSPRT();
sacUpdateSPRT(p); updateSPRT();
if(sacIsBestModel(p)){ if(isBestModel()){
sacSaveBestModel(p); saveBestModel();
if(sacIsRefineEnabled(p) && sacCanRefine(p)){ if(isRefineEnabled() && canRefine()){
sacRefine(p); refine();
} }
sacUpdateBounds(p); updateBounds();
if(sacIsNREnabled(p)){ if(isNREnabled()){
sacNStarOptimize(p); nStarOptimize();
} }
} }
...@@ -676,8 +875,8 @@ static inline int sacVerify(RHO_HEST_REFC* p){ ...@@ -676,8 +875,8 @@ static inline int sacVerify(RHO_HEST_REFC* p){
* @return Zero if no extrinsic guess was provided; non-zero otherwiseEE. * @return Zero if no extrinsic guess was provided; non-zero otherwiseEE.
*/ */
static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::haveExtrinsicGuess(void){
return !!p->arg.guessH; return !!arg.guessH;
} }
/** /**
...@@ -686,8 +885,8 @@ static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p){ ...@@ -686,8 +885,8 @@ static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p){
* @return Zero if non-randomness criterion disabled; non-zero if not. * @return Zero if non-randomness criterion disabled; non-zero if not.
*/ */
static inline int sacIsNREnabled(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isNREnabled(void){
return p->arg.flags & RHO_FLAG_ENABLE_NR; return arg.flags & RHO_FLAG_ENABLE_NR;
} }
/** /**
...@@ -696,8 +895,8 @@ static inline int sacIsNREnabled(RHO_HEST_REFC* p){ ...@@ -696,8 +895,8 @@ static inline int sacIsNREnabled(RHO_HEST_REFC* p){
* @return Zero if best-model-so-far refinement disabled; non-zero if not. * @return Zero if best-model-so-far refinement disabled; non-zero if not.
*/ */
static inline int sacIsRefineEnabled(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isRefineEnabled(void){
return p->arg.flags & RHO_FLAG_ENABLE_REFINEMENT; return arg.flags & RHO_FLAG_ENABLE_REFINEMENT;
} }
/** /**
...@@ -706,8 +905,8 @@ static inline int sacIsRefineEnabled(RHO_HEST_REFC* p){ ...@@ -706,8 +905,8 @@ static inline int sacIsRefineEnabled(RHO_HEST_REFC* p){
* @return Zero if final-model refinement disabled; non-zero if not. * @return Zero if final-model refinement disabled; non-zero if not.
*/ */
static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isFinalRefineEnabled(void){
return p->arg.flags & RHO_FLAG_ENABLE_FINAL_REFINEMENT; return arg.flags & RHO_FLAG_ENABLE_FINAL_REFINEMENT;
} }
/** /**
...@@ -718,8 +917,8 @@ static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p){ ...@@ -718,8 +917,8 @@ static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p){
* Read: i, phEndI, phNum, phMax. * Read: i, phEndI, phNum, phMax.
*/ */
static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::PROSACPhaseEndReached(void){
return p->ctrl.i >= p->ctrl.phEndI && p->ctrl.phNum < p->ctrl.phMax; return ctrl.i >= ctrl.phEndI && ctrl.phNum < ctrl.phMax;
} }
/** /**
...@@ -733,13 +932,13 @@ static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p){ ...@@ -733,13 +932,13 @@ static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p){
* Write: phNum, phEndFpI, phEndI * Write: phNum, phEndFpI, phEndI
*/ */
static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::PROSACGoToNextPhase(void){
double next; double next;
p->ctrl.phNum++; ctrl.phNum++;
next = (p->ctrl.phEndFpI * p->ctrl.phNum)/(p->ctrl.phNum - m); next = (ctrl.phEndFpI * ctrl.phNum)/(ctrl.phNum - SMPL_SIZE);
p->ctrl.phEndI += (unsigned)ceil(next - p->ctrl.phEndFpI); ctrl.phEndI += (unsigned)ceil(next - ctrl.phEndFpI);
p->ctrl.phEndFpI = next; ctrl.phEndFpI = next;
} }
/** /**
...@@ -750,12 +949,13 @@ static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){ ...@@ -750,12 +949,13 @@ static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){
* the first phNum-1 matches. * the first phNum-1 matches.
*/ */
static inline void sacGetPROSACSample(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::getPROSACSample(void){
if(p->ctrl.i > p->ctrl.phEndI){ if(ctrl.i > ctrl.phEndI){
sacRndSmpl(4, p->ctrl.smpl, p->ctrl.phNum);/* Used to be phMax */ /* FIXME: Dubious. Review. */
sacRndSmpl(4, ctrl.smpl, ctrl.phNum);/* Used to be phMax */
}else{ }else{
sacRndSmpl(3, p->ctrl.smpl, p->ctrl.phNum-1); sacRndSmpl(3, ctrl.smpl, ctrl.phNum-1);
p->ctrl.smpl[3] = p->ctrl.phNum-1; ctrl.smpl[3] = ctrl.phNum-1;
} }
} }
...@@ -767,10 +967,10 @@ static inline void sacGetPROSACSample(RHO_HEST_REFC* p){ ...@@ -767,10 +967,10 @@ static inline void sacGetPROSACSample(RHO_HEST_REFC* p){
* bad samples. * bad samples.
*/ */
static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isSampleDegenerate(void){
unsigned i0 = p->ctrl.smpl[0], i1 = p->ctrl.smpl[1], i2 = p->ctrl.smpl[2], i3 = p->ctrl.smpl[3]; unsigned i0 = ctrl.smpl[0], i1 = ctrl.smpl[1], i2 = ctrl.smpl[2], i3 = ctrl.smpl[3];
typedef struct{float x,y;} MyPt2f; typedef struct{float x,y;} MyPt2f;
MyPt2f* pkdPts = (MyPt2f*)p->curr.pkdPts, *src = (MyPt2f*)p->arg.src, *dst = (MyPt2f*)p->arg.dst; MyPt2f* pkdPts = (MyPt2f*)curr.pkdPts, *src = (MyPt2f*)arg.src, *dst = (MyPt2f*)arg.dst;
/** /**
* Pack the matches selected by the SAC algorithm. * Pack the matches selected by the SAC algorithm.
...@@ -860,8 +1060,8 @@ static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p){ ...@@ -860,8 +1060,8 @@ static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p){
* current homography. * current homography.
*/ */
static inline void sacGenerateModel(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::generateModel(void){
hFuncRefC(p->curr.pkdPts, p->curr.H); hFuncRefC(curr.pkdPts, curr.H);
} }
/** /**
...@@ -870,9 +1070,9 @@ static inline void sacGenerateModel(RHO_HEST_REFC* p){ ...@@ -870,9 +1070,9 @@ static inline void sacGenerateModel(RHO_HEST_REFC* p){
* NaN the homography is rejected. * NaN the homography is rejected.
*/ */
static inline int sacIsModelDegenerate(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isModelDegenerate(void){
int degenerate; int degenerate;
float* H = p->curr.H; float* H = curr.H;
float f=H[0]+H[1]+H[2]+H[3]+H[4]+H[5]+H[6]+H[7]; float f=H[0]+H[1]+H[2]+H[3]+H[4]+H[5]+H[6]+H[7];
/* degenerate = isnan(f); */ /* degenerate = isnan(f); */
...@@ -890,26 +1090,26 @@ static inline int sacIsModelDegenerate(RHO_HEST_REFC* p){ ...@@ -890,26 +1090,26 @@ static inline int sacIsModelDegenerate(RHO_HEST_REFC* p){
* Writes: eval.*, curr.inl, curr.numInl * Writes: eval.*, curr.inl, curr.numInl
*/ */
static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::evaluateModelSPRT(void){
unsigned i; unsigned i;
unsigned isInlier; unsigned isInlier;
double lambda = 1.0; double lambda = 1.0;
float distSq = p->arg.maxD*p->arg.maxD; float distSq = arg.maxD*arg.maxD;
const float* src = p->arg.src; const float* src = arg.src;
const float* dst = p->arg.dst; const float* dst = arg.dst;
char* inl = p->curr.inl; char* inl = curr.inl;
const float* H = p->curr.H; const float* H = curr.H;
p->ctrl.numModels++; ctrl.numModels++;
p->curr.numInl = 0; curr.numInl = 0;
p->eval.Ntested = 0; eval.Ntested = 0;
p->eval.good = 1; eval.good = 1;
/* SCALAR */ /* SCALAR */
for(i=0;i<p->arg.N && p->eval.good;i++){ for(i=0;i<arg.N && eval.good;i++){
/* Backproject */ /* Backproject */
float x=src[i*2],y=src[i*2+1]; float x=src[i*2],y=src[i*2+1];
float X=dst[i*2],Y=dst[i*2+1]; float X=dst[i*2],Y=dst[i*2+1];
...@@ -931,19 +1131,19 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ ...@@ -931,19 +1131,19 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){
/* ... */ /* ... */
isInlier = reprojDist <= distSq; isInlier = reprojDist <= distSq;
p->curr.numInl += isInlier; curr.numInl += isInlier;
*inl++ = (char)isInlier; *inl++ = (char)isInlier;
/* SPRT */ /* SPRT */
lambda *= isInlier ? p->eval.lambdaAccept : p->eval.lambdaReject; lambda *= isInlier ? eval.lambdaAccept : eval.lambdaReject;
p->eval.good = lambda <= p->eval.A; eval.good = lambda <= eval.A;
/* If !p->good, the threshold A was exceeded, so we're rejecting */ /* If !good, the threshold A was exceeded, so we're rejecting */
} }
p->eval.Ntested = i; eval.Ntested = i;
p->eval.Ntestedtotal += i; eval.Ntestedtotal += i;
} }
/** /**
...@@ -959,18 +1159,21 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ ...@@ -959,18 +1159,21 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){
* eval.lambdaReject * eval.lambdaReject
*/ */
static inline void sacUpdateSPRT(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::updateSPRT(void){
if(p->eval.good){ if(eval.good){
if(sacIsBestModel(p)){ if(isBestModel()){
p->eval.epsilon = (double)p->curr.numInl/p->arg.N; eval.epsilon = (double)curr.numInl/arg.N;
sacDesignSPRTTest(p); designSPRTTest();
} }
}else{ }else{
double newDelta = (double)p->curr.numInl/p->eval.Ntested; double newDelta = (double)curr.numInl/eval.Ntested;
if(newDelta > 0 && CHNG_SIGNIFICANT(p->eval.delta, newDelta)){ if(newDelta > 0){
p->eval.delta = newDelta; double relChange = fabs(eval.delta - newDelta)/ eval.delta;
sacDesignSPRTTest(p); if(relChange > MIN_DELTA_CHNG){
eval.delta = newDelta;
designSPRTTest();
}
} }
} }
} }
...@@ -1044,18 +1247,18 @@ static inline double designSPRTTest(double delta, double epsilon, double t_M, do ...@@ -1044,18 +1247,18 @@ static inline double designSPRTTest(double delta, double epsilon, double t_M, do
* Writes: eval.A, eval.lambdaAccept, eval.lambdaReject * Writes: eval.A, eval.lambdaAccept, eval.lambdaReject
*/ */
static inline void sacDesignSPRTTest(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::designSPRTTest(void){
p->eval.A = designSPRTTest(p->eval.delta, p->eval.epsilon, p->eval.t_M, p->eval.m_S); eval.A = designSPRTTest(eval.delta, eval.epsilon, eval.t_M, eval.m_S);
p->eval.lambdaReject = ((1.0 - p->eval.delta) / (1.0 - p->eval.epsilon)); eval.lambdaReject = ((1.0 - eval.delta) / (1.0 - eval.epsilon));
p->eval.lambdaAccept = (( p->eval.delta ) / ( p->eval.epsilon )); eval.lambdaAccept = (( eval.delta ) / ( eval.epsilon ));
} }
/** /**
* Return whether the current model is the best model so far. * Return whether the current model is the best model so far.
*/ */
static inline int sacIsBestModel(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isBestModel(void){
return p->curr.numInl > p->best.numInl; return curr.numInl > best.numInl;
} }
/** /**
...@@ -1064,8 +1267,8 @@ static inline int sacIsBestModel(RHO_HEST_REFC* p){ ...@@ -1064,8 +1267,8 @@ static inline int sacIsBestModel(RHO_HEST_REFC* p){
* number of inliers. * number of inliers.
*/ */
static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::isBestModelGoodEnough(void){
return p->best.numInl >= p->arg.minInl; return best.numInl >= arg.minInl;
} }
/** /**
...@@ -1073,18 +1276,18 @@ static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){ ...@@ -1073,18 +1276,18 @@ static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){
* and count of inliers between the current and best models. * and count of inliers between the current and best models.
*/ */
static inline void sacSaveBestModel(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::saveBestModel(void){
float* H = p->curr.H; float* H = curr.H;
char* inl = p->curr.inl; char* inl = curr.inl;
unsigned numInl = p->curr.numInl; unsigned numInl = curr.numInl;
p->curr.H = p->best.H; curr.H = best.H;
p->curr.inl = p->best.inl; curr.inl = best.inl;
p->curr.numInl = p->best.numInl; curr.numInl = best.numInl;
p->best.H = H; best.H = H;
p->best.inl = inl; best.inl = inl;
p->best.numInl = numInl; best.numInl = numInl;
} }
/** /**
...@@ -1095,13 +1298,13 @@ static inline void sacInitNonRand(double beta, ...@@ -1095,13 +1298,13 @@ static inline void sacInitNonRand(double beta,
unsigned start, unsigned start,
unsigned N, unsigned N,
unsigned* nonRandMinInl){ unsigned* nonRandMinInl){
unsigned n = m+1 > start ? m+1 : start; unsigned n = SMPL_SIZE+1 > start ? SMPL_SIZE+1 : start;
double beta_beta1_sq_chi = sqrt(beta*(1.0-beta)) * CHI_SQ; double beta_beta1_sq_chi = sqrt(beta*(1.0-beta)) * CHI_SQ;
for(; n < N; n++){ for(; n < N; n++){
double mu = n * beta; double mu = n * beta;
double sigma = sqrt(n)* beta_beta1_sq_chi; double sigma = sqrt(n)* beta_beta1_sq_chi;
unsigned i_min = (unsigned)ceil(m + mu + sigma); unsigned i_min = (unsigned)ceil(SMPL_SIZE + mu + sigma);
nonRandMinInl[n] = i_min; nonRandMinInl[n] = i_min;
} }
...@@ -1112,31 +1315,31 @@ static inline void sacInitNonRand(double beta, ...@@ -1112,31 +1315,31 @@ static inline void sacInitNonRand(double beta,
* of PROSAC. * of PROSAC.
*/ */
static inline void sacNStarOptimize(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::nStarOptimize(void){
unsigned min_sample_length = 10*2; /*(p->N * INLIERS_RATIO) */ unsigned min_sample_length = 10*2; /*(N * INLIERS_RATIO) */
unsigned best_n = p->arg.N; unsigned best_n = arg.N;
unsigned test_n = best_n; unsigned test_n = best_n;
unsigned bestNumInl = p->best.numInl; unsigned bestNumInl = best.numInl;
unsigned testNumInl = bestNumInl; unsigned testNumInl = bestNumInl;
for(;test_n > min_sample_length && testNumInl;test_n--){ for(;test_n > min_sample_length && testNumInl;test_n--){
if(testNumInl*best_n > bestNumInl*test_n){ if(testNumInl*best_n > bestNumInl*test_n){
if(testNumInl < p->nr.tbl[test_n]){ if(testNumInl < nr.tbl[test_n]){
break; break;
} }
best_n = test_n; best_n = test_n;
bestNumInl = testNumInl; bestNumInl = testNumInl;
} }
testNumInl -= !!p->arg.inl[test_n-1]; testNumInl -= !!arg.inl[test_n-1];
} }
if(bestNumInl*p->ctrl.phMax > p->ctrl.phNumInl*best_n){ if(bestNumInl*ctrl.phMax > ctrl.phNumInl*best_n){
p->ctrl.phMax = best_n; ctrl.phMax = best_n;
p->ctrl.phNumInl = bestNumInl; ctrl.phNumInl = bestNumInl;
p->arg.maxI = sacCalcIterBound(p->arg.cfd, arg.maxI = sacCalcIterBound(arg.cfd,
(double)p->ctrl.phNumInl/p->ctrl.phMax, (double)ctrl.phNumInl/ctrl.phMax,
m, SMPL_SIZE,
p->arg.maxI); arg.maxI);
} }
} }
...@@ -1144,25 +1347,25 @@ static inline void sacNStarOptimize(RHO_HEST_REFC* p){ ...@@ -1144,25 +1347,25 @@ static inline void sacNStarOptimize(RHO_HEST_REFC* p){
* Classic RANSAC iteration bound based on largest # of inliers. * Classic RANSAC iteration bound based on largest # of inliers.
*/ */
static inline void sacUpdateBounds(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::updateBounds(void){
p->arg.maxI = sacCalcIterBound(p->arg.cfd, arg.maxI = sacCalcIterBound(arg.cfd,
(double)p->best.numInl/p->arg.N, (double)best.numInl/arg.N,
m, SMPL_SIZE,
p->arg.maxI); arg.maxI);
} }
/** /**
* Ouput the best model so far to the output argument. * Ouput the best model so far to the output argument.
*/ */
static inline void sacOutputModel(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::outputModel(void){
if(sacIsBestModelGoodEnough(p)){ if(isBestModelGoodEnough()){
memcpy(p->arg.finalH, p->best.H, HSIZE); memcpy(arg.finalH, best.H, HSIZE);
if(p->arg.inl != p->best.inl){ if(arg.inl != best.inl){
memcpy(p->arg.inl, p->best.inl, p->arg.N); memcpy(arg.inl, best.inl, arg.N);
} }
}else{ }else{
sacOutputZeroH(p); outputZeroH();
} }
} }
...@@ -1170,8 +1373,9 @@ static inline void sacOutputModel(RHO_HEST_REFC* p){ ...@@ -1170,8 +1373,9 @@ static inline void sacOutputModel(RHO_HEST_REFC* p){
* Ouput a zeroed H to the output argument. * Ouput a zeroed H to the output argument.
*/ */
static inline void sacOutputZeroH(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::outputZeroH(void){
memset(p->arg.finalH, 0, HSIZE); memset(arg.finalH, 0, HSIZE);
memset(arg.inl, 0, arg.N);
} }
/** /**
...@@ -1572,20 +1776,20 @@ static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) ...@@ -1572,20 +1776,20 @@ static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates)
* NB This is separate from whether it is *enabled*. * NB This is separate from whether it is *enabled*.
*/ */
static inline int sacCanRefine(RHO_HEST_REFC* p){ inline int RHO_HEST_REFC::canRefine(void){
/** /**
* If we only have 4 matches, GE's result is already optimal and cannot * If we only have 4 matches, GE's result is already optimal and cannot
* be refined any further. * be refined any further.
*/ */
return p->best.numInl > m; return best.numInl > SMPL_SIZE;
} }
/** /**
* Refines the best-so-far homography (p->best.H). * Refines the best-so-far homography (p->best.H).
*/ */
static inline void sacRefine(RHO_HEST_REFC* p){ inline void RHO_HEST_REFC::refine(void){
int i; int i;
float S, newS; /* Sum of squared errors */ float S, newS; /* Sum of squared errors */
float gain; /* Gain-parameter. */ float gain; /* Gain-parameter. */
...@@ -1597,8 +1801,8 @@ static inline void sacRefine(RHO_HEST_REFC* p){ ...@@ -1597,8 +1801,8 @@ static inline void sacRefine(RHO_HEST_REFC* p){
*/ */
/* Find initial conditions */ /* Find initial conditions */
sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, sacCalcJacobianErrors(best.H, arg.src, arg.dst, arg.inl, arg.N,
p->lm.JtJ, p->lm.Jte, &S); lm.JtJ, lm.Jte, &S);
/*Levenberg-Marquardt Loop.*/ /*Levenberg-Marquardt Loop.*/
for(i=0;i<MAXLEVMARQITERS;i++){ for(i=0;i<MAXLEVMARQITERS;i++){
...@@ -1623,15 +1827,15 @@ static inline void sacRefine(RHO_HEST_REFC* p){ ...@@ -1623,15 +1827,15 @@ static inline void sacRefine(RHO_HEST_REFC* p){
* transpose) then multiply Jte in order to find dH. * transpose) then multiply Jte in order to find dH.
*/ */
while(!sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1)){ while(!sacChol8x8Damped(lm.JtJ, L, lm.tmp1)){
L *= 2.0f; L *= 2.0f;
} }
sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1); sacTRInv8x8 (lm.tmp1, lm.tmp1);
sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); sacTRISolve8x8(lm.tmp1, lm.Jte, dH);
sacSub8x1 (newH, p->best.H, dH); sacSub8x1 (newH, best.H, dH);
sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, sacCalcJacobianErrors(newH, arg.src, arg.dst, arg.inl, arg.N,
NULL, NULL, &newS); NULL, NULL, &newS);
gain = sacLMGain(dH, p->lm.Jte, S, newS, L); gain = sacLMGain(dH, lm.Jte, S, newS, L);
/*printf("Lambda: %12.6f S: %12.6f newS: %12.6f Gain: %12.6f\n", /*printf("Lambda: %12.6f S: %12.6f newS: %12.6f Gain: %12.6f\n",
L, S, newS, gain);*/ L, S, newS, gain);*/
...@@ -1656,9 +1860,9 @@ static inline void sacRefine(RHO_HEST_REFC* p){ ...@@ -1656,9 +1860,9 @@ static inline void sacRefine(RHO_HEST_REFC* p){
if(gain > 0){ if(gain > 0){
S = newS; S = newS;
memcpy(p->best.H, newH, sizeof(newH)); memcpy(best.H, newH, sizeof(newH));
sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, sacCalcJacobianErrors(best.H, arg.src, arg.dst, arg.inl, arg.N,
p->lm.JtJ, p->lm.Jte, &S); lm.JtJ, lm.Jte, &S);
} }
} }
} }
...@@ -2165,7 +2369,5 @@ static inline void sacSub8x1(float* Hout, const float* H, const float* dH){ ...@@ -2165,7 +2369,5 @@ static inline void sacSub8x1(float* Hout, const float* H, const float* dH){
} }
/* End namespace cv */
#ifdef __cplusplus
} }
#endif
modules/calib3d/src/rhorefc.h
View file @ adac8c04
...@@ -56,7 +56,6 @@ ...@@ -56,7 +56,6 @@
/* Defines */ /* Defines */
#ifdef __cplusplus
/* C++ does not have the restrict keyword. */ /* C++ does not have the restrict keyword. */
#ifdef restrict #ifdef restrict
...@@ -64,15 +63,6 @@ ...@@ -64,15 +63,6 @@
#endif #endif
#define restrict #define restrict
#else
/* C99 and over has the restrict keyword. */
#if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L
#define restrict
#endif
#endif
/* Flags */ /* Flags */
#ifndef RHO_FLAG_NONE #ifndef RHO_FLAG_NONE
...@@ -90,101 +80,17 @@ ...@@ -90,101 +80,17 @@
/* Namespace cv */
namespace cv{
/* Data structures */ /* Data structures */
/** /**
* Homography Estimation context. * Homography Estimation context.
*/ */
typedef struct{ struct RHO_HEST_REFC;
/** typedef struct RHO_HEST_REFC RHO_HEST_REFC;
* Virtual Arguments.
*
* Exactly the same as at function call, except:
* - minInl is enforced to be >= 4.
*/
struct{
const float* restrict src;
const float* restrict dst;
char* restrict inl;
unsigned N;
float maxD;
unsigned maxI;
unsigned rConvg;
double cfd;
unsigned minInl;
double beta;
unsigned flags;
const float* guessH;
float* finalH;
} arg;
/* PROSAC Control */
struct{
unsigned i; /* Iteration Number */
unsigned phNum; /* Phase Number */
unsigned phEndI; /* Phase End Iteration */
double phEndFpI; /* Phase floating-point End Iteration */
unsigned phMax; /* Termination phase number */
unsigned phNumInl; /* Number of inliers for termination phase */
unsigned numModels; /* Number of models tested */
unsigned* restrict smpl; /* Sample of match indexes */
} ctrl;
/* Current model being tested */
struct{
float* restrict pkdPts; /* Packed points */
float* restrict H; /* Homography */
char* restrict inl; /* Mask of inliers */
unsigned numInl; /* Number of inliers */
} curr;
/* Best model (so far) */
struct{
float* restrict H; /* Homography */
char* restrict inl; /* Mask of inliers */
unsigned numInl; /* Number of inliers */
} best;
/* Non-randomness criterion */
struct{
unsigned* restrict tbl; /* Non-Randomness: Table */
unsigned size; /* Non-Randomness: Size */
double beta; /* Non-Randomness: Beta */
} nr;
/* SPRT Evaluator */
struct{
double t_M; /* t_M */
double m_S; /* m_S */
double epsilon; /* Epsilon */
double delta; /* delta */
double A; /* SPRT Threshold */
unsigned Ntested; /* Number of points tested */
unsigned Ntestedtotal; /* Number of points tested in total */
int good; /* Good/bad flag */
double lambdaAccept; /* Accept multiplier */
double lambdaReject; /* Reject multiplier */
} eval;
/* Levenberg-Marquardt Refinement */
struct{
float* ws; /* Levenberg-Marqhard Workspace */
float (* restrict JtJ)[8]; /* JtJ matrix */
float (* restrict tmp1)[8]; /* Temporary 1 */
float* restrict Jte; /* Jte vector */
} lm;
} RHO_HEST_REFC;
/* Extern C */
#ifdef __cplusplus
namespace cv{
/* extern "C" { */
#endif
/* Functions */ /* Functions */
...@@ -193,11 +99,10 @@ namespace cv{ ...@@ -193,11 +99,10 @@ namespace cv{
* Initialize the estimator context, by allocating the aligned buffers * Initialize the estimator context, by allocating the aligned buffers
* internally needed. * internally needed.
* *
* @param [in/out] p The uninitialized estimator context to initialize. * @return A pointer to the context if successful; NULL if an error occured.
* @return 0 if successful; non-zero if an error occured.
*/ */
int rhoRefCInit(RHO_HEST_REFC* p); RHO_HEST_REFC* rhoRefCInit(void);
/** /**
...@@ -355,11 +260,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte ...@@ -355,11 +260,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte
/* Extern C */ /* End Namespace cv */
#ifdef __cplusplus
/* } *//* End extern "C" */
} }
#endif
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment