WAIFrame Class Reference

#include <WAIFrame.h>

Public Member Functions
	WAIFrame ()
	WAIFrame (const WAIFrame &frame)
	copy constructor More...
	WAIFrame (const cv::Mat &imGray, const double &timeStamp, KPextractor *extractor, cv::Mat &K, cv::Mat &distCoef, WAIOrbVocabulary *vocabulary, bool retainImg=false)
	constructor used for detection in tracking More...
void	ExtractFeaturePoints (const cv::Mat &im)
void	ComputeBoW ()
void	SetPose (cv::Mat Tcw)
void	UpdatePoseMatrices ()
cv::Mat	GetCameraCenter ()
cv::Mat	GetRotationInverse ()
cv::Mat	GetTranslationCW ()
cv::Mat	GetRotationCW ()
bool	isInFrustum (WAIMapPoint *pMP, float viewingCosLimit)
bool	PosInGrid (const cv::KeyPoint &kp, int &posX, int &posY)
std::vector< size_t >	GetFeaturesInArea (const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1) const

Public Attributes
WAIOrbVocabulary *	mVocabulary = NULL
KPextractor *	mpORBextractorLeft = NULL
double	mTimeStamp
cv::Mat	mK
cv::Mat	mDistCoef
int	N = 0
std::vector< cv::KeyPoint >	mvKeys
std::vector< cv::KeyPoint >	mvKeysUn
std::vector< float >	mvuRight
std::vector< float >	mvDepth
WAIBowVector	mBowVec
WAIFeatVector	mFeatVec
cv::Mat	mDescriptors
std::vector< WAIMapPoint * >	mvpMapPoints
std::vector< std::size_t >	mGrid [64][36]
cv::Mat	mTcw
long unsigned int	mnId = -1
WAIKeyFrame *	mpReferenceKF = NULL
int	mnScaleLevels
float	mfScaleFactor
float	mfLogScaleFactor
std::vector< float >	mvScaleFactors
std::vector< float >	mvInvScaleFactors
std::vector< float >	mvLevelSigma2
std::vector< float >	mvInvLevelSigma2
cv::Mat	imgGray

Static Public Attributes
static float	fx = 0.0f
static float	fy = 0.0f
static float	cx = 0.0f
static float	cy = 0.0f
static float	invfx = 0.0f
static float	invfy = 0.0f
static float	mfGridElementWidthInv = 0.0f
static float	mfGridElementHeightInv = 0.0f
static long unsigned int	nNextId = 0
static float	mnMinX = 0.0f
static float	mnMaxX = 0.0f
static float	mnMinY = 0.0f
static float	mnMaxY = 0.0f
static bool	mbInitialComputations = true

Private Member Functions
void	UndistortKeyPoints ()
void	ComputeImageBounds (const cv::Mat &imLeft)
void	AssignFeaturesToGrid ()

Private Attributes
cv::Mat	mRcw
cv::Mat	mtcw
cv::Mat	mRwc
cv::Mat	mOw

Detailed Description

Definition at line 48 of file WAIFrame.h.

Constructor & Destructor Documentation

WAIFrame() [1/3]

WAIFrame::WAIFrame

(

)

Definition at line 53 of file WAIFrame.cpp.

{
}

WAIFrame() [2/3]

WAIFrame::WAIFrame

(

const WAIFrame &

frame

)

copy constructor

Definition at line 58 of file WAIFrame.cpp.

  : mVocabulary(frame.mVocabulary),
    mpORBextractorLeft(frame.mpORBextractorLeft),
    mTimeStamp(frame.mTimeStamp),
    mK(frame.mK.clone()),
    mDistCoef(frame.mDistCoef.clone()),
    N(frame.N),
    mvKeys(frame.mvKeys),
    mvKeysUn(frame.mvKeysUn), /*mvuRight(frame.mvuRight),
    mvDepth(frame.mvDepth),*/
    mBowVec(frame.mBowVec),
    mFeatVec(frame.mFeatVec),
    mDescriptors(frame.mDescriptors.clone()),
    mvpMapPoints(frame.mvpMapPoints),
    mnId(frame.mnId),
    mpReferenceKF(frame.mpReferenceKF),
    mnScaleLevels(frame.mnScaleLevels),
    mfScaleFactor(frame.mfScaleFactor),
    mfLogScaleFactor(frame.mfLogScaleFactor),
    mvScaleFactors(frame.mvScaleFactors),
    mvInvScaleFactors(frame.mvInvScaleFactors),
    mvLevelSigma2(frame.mvLevelSigma2),
    mvInvLevelSigma2(frame.mvInvLevelSigma2)
{
    for (int i = 0; i < FRAME_GRID_COLS; i++)
        for (int j = 0; j < FRAME_GRID_ROWS; j++)
            mGrid[i][j] = frame.mGrid[i][j];
 
    if (!frame.mTcw.empty())
        SetPose(frame.mTcw);
 
    if (!frame.imgGray.empty())
        imgGray = frame.imgGray.clone();
}

WAIFrame() [3/3]

WAIFrame::WAIFrame	(	const cv::Mat &	imGray,
		const double &	timeStamp,
		KPextractor *	extractor,
		cv::Mat &	K,
		cv::Mat &	distCoef,
		WAIOrbVocabulary *	vocabulary,
		bool	retainImg = false
	)

constructor used for detection in tracking

Definition at line 93 of file WAIFrame.cpp.

  : mpORBextractorLeft(extractor), mTimeStamp(timeStamp), /*mK(K.clone()),*/ /*mDistCoef(distCoef.clone()),*/
    mVocabulary(vocabulary)
{
    AVERAGE_TIMING_START("WAIFrame");
    //ghm1: ORB_SLAM uses float precision
    K.convertTo(mK, CV_32F);
    distCoef.convertTo(mDistCoef, CV_32F);
 
    // Frame ID
    mnId = nNextId++;
 
    // Scale Level Info
    mnScaleLevels     = mpORBextractorLeft->GetLevels();
    mfScaleFactor     = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor  = log(mfScaleFactor);
    mvScaleFactors    = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2     = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2  = mpORBextractorLeft->GetInverseScaleSigmaSquares();
 
    // ORB extraction
    ExtractFeaturePoints(imGray);
 
    N = (int)mvKeys.size();
 
    if (mvKeys.empty())
    {
        AVERAGE_TIMING_STOP("WAIFrame");
        return;
    }
 
    UndistortKeyPoints();
 
    mvpMapPoints = vector<WAIMapPoint*>(N, static_cast<WAIMapPoint*>(NULL));
 
    // This is done only for the first Frame (or after a change in the calibration)
    if (mbInitialComputations)
    {
        ComputeImageBounds(imGray);
 
        mfGridElementWidthInv  = static_cast<float>(FRAME_GRID_COLS) / (mnMaxX - mnMinX);
        mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) / (mnMaxY - mnMinY);
 
        fx    = mK.at<float>(0, 0);
        fy    = mK.at<float>(1, 1);
        cx    = mK.at<float>(0, 2);
        cy    = mK.at<float>(1, 2);
        invfx = 1.0f / fx;
        invfy = 1.0f / fy;
 
        mbInitialComputations = false;
    }
 
    AssignFeaturesToGrid();
 
    //store image reference if required
    if (retainImg)
        imgGray = imGray.clone();
 
    AVERAGE_TIMING_STOP("WAIFrame");
}

Member Function Documentation

AssignFeaturesToGrid()

void WAIFrame::AssignFeaturesToGrid ( )

private

Definition at line 156 of file WAIFrame.cpp.

{
    int nReserve = (int)(0.5f * N / (FRAME_GRID_COLS * FRAME_GRID_ROWS));
    for (unsigned int i = 0; i < FRAME_GRID_COLS; i++)
        for (unsigned int j = 0; j < FRAME_GRID_ROWS; j++)
            mGrid[i][j].reserve(nReserve);
 
    for (int i = 0; i < N; i++)
    {
        const cv::KeyPoint& kp = mvKeysUn[i];
 
        int nGridPosX, nGridPosY;
        if (PosInGrid(kp, nGridPosX, nGridPosY))
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}

ComputeBoW()

void WAIFrame::ComputeBoW

(

)

Definition at line 317 of file WAIFrame.cpp.

{
    if (!mBowVec.isFill)
    {
        //vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        // Luc: In a 6 levels and 10 branch per level voc, 4 levelup mean the 2nd level from the top
        // that make a total of 100 words. More words means more variance between keyframe and less
        // preselected keyframe but that will make also the relocalization less invariant to changes
 
        mVocabulary->transform(mDescriptors, mBowVec, mFeatVec);
    }
}

ComputeImageBounds()

void WAIFrame::ComputeImageBounds ( const cv::Mat &  imLeft )

private

Definition at line 362 of file WAIFrame.cpp.

{
    if (mDistCoef.at<float>(0) != 0.0)
    {
        cv::Mat mat(4, 2, CV_32F);
        mat.at<float>(0, 0) = 0.0;
        mat.at<float>(0, 1) = 0.0;
        mat.at<float>(1, 0) = (float)imLeft.cols;
        mat.at<float>(1, 1) = 0.0;
        mat.at<float>(2, 0) = 0.0;
        mat.at<float>(2, 1) = (float)imLeft.rows;
        mat.at<float>(3, 0) = (float)imLeft.cols;
        mat.at<float>(3, 1) = (float)imLeft.rows;
 
        // Undistort corners
        mat = mat.reshape(2);
        cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);
        mat = mat.reshape(1);
 
        mnMinX = (float)min(mat.at<float>(0, 0), mat.at<float>(2, 0));
        mnMaxX = (float)max(mat.at<float>(1, 0), mat.at<float>(3, 0));
        mnMinY = (float)min(mat.at<float>(0, 1), mat.at<float>(1, 1));
        mnMaxY = (float)max(mat.at<float>(2, 1), mat.at<float>(3, 1));
    }
    else
    {
        mnMinX = 0.0f;
        mnMaxX = (float)imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = (float)imLeft.rows;
    }
}

ExtractFeaturePoints()

void WAIFrame::ExtractFeaturePoints

(

const cv::Mat &

im

)

Definition at line 173 of file WAIFrame.cpp.

{
    (*mpORBextractorLeft)(im, mvKeys, mDescriptors);
}

GetCameraCenter()

cv::Mat WAIFrame::GetCameraCenter ( )

inline

Definition at line 70 of file WAIFrame.h.

    {
        return mOw.clone();
    }

GetFeaturesInArea()

vector< size_t > WAIFrame::GetFeaturesInArea	(	const float &	x,
		const float &	y,
		const float &	r,
		const int	minLevel = -1,
		const int	maxLevel = -1
	)		const

Definition at line 250 of file WAIFrame.cpp.

{
    vector<size_t> vIndices;
    vIndices.reserve(N);
 
    const int nMinCellX = max(0, (int)floor((x - mnMinX - r) * mfGridElementWidthInv));
    if (nMinCellX >= FRAME_GRID_COLS)
        return vIndices;
 
    const int nMaxCellX = min((int)FRAME_GRID_COLS - 1, (int)ceil((x - mnMinX + r) * mfGridElementWidthInv));
    if (nMaxCellX < 0)
        return vIndices;
 
    const int nMinCellY = max(0, (int)floor((y - mnMinY - r) * mfGridElementHeightInv));
    if (nMinCellY >= FRAME_GRID_ROWS)
        return vIndices;
 
    const int nMaxCellY = min((int)FRAME_GRID_ROWS - 1, (int)ceil((y - mnMinY + r) * mfGridElementHeightInv));
    if (nMaxCellY < 0)
        return vIndices;
 
    const bool bCheckLevels = (minLevel > 0) || (maxLevel >= 0);
 
    for (int ix = nMinCellX; ix <= nMaxCellX; ix++)
    {
        for (int iy = nMinCellY; iy <= nMaxCellY; iy++)
        {
            const vector<size_t> vCell = mGrid[ix][iy];
            if (vCell.empty())
                continue;
 
            for (size_t j = 0, jend = vCell.size(); j < jend; j++)
            {
                const cv::KeyPoint& kpUn = mvKeysUn[vCell[j]];
                if (bCheckLevels)
                {
                    if (kpUn.octave < minLevel)
                        continue;
                    if (maxLevel >= 0)
                        if (kpUn.octave > maxLevel)
                            continue;
                }
 
                const float distx = kpUn.pt.x - x;
                const float disty = kpUn.pt.y - y;
 
                if (fabs(distx) < r && fabs(disty) < r)
                    vIndices.push_back(vCell[j]);
            }
        }
    }
 
    return vIndices;
}

GetRotationCW()

cv::Mat WAIFrame::GetRotationCW ( )

inline

Definition at line 88 of file WAIFrame.h.

    {
        return mRcw.clone();
    }

GetRotationInverse()

cv::Mat WAIFrame::GetRotationInverse ( )

inline

Definition at line 76 of file WAIFrame.h.

    {
        return mRwc.clone();
    }

GetTranslationCW()

cv::Mat WAIFrame::GetTranslationCW ( )

inline

Definition at line 82 of file WAIFrame.h.

    {
        return mtcw.clone();
    }

isInFrustum()

bool WAIFrame::isInFrustum	(	WAIMapPoint *	pMP,
		float	viewingCosLimit
	)

Definition at line 192 of file WAIFrame.cpp.

{
    pMP->mbTrackInView = false;
 
    // 3D in absolute coordinates
    cv::Mat P = pMP->GetWorldPos();
 
    // 3D in camera coordinates
    const cv::Mat Pc  = mRcw * P + mtcw;
    const float&  PcX = Pc.at<float>(0);
    const float&  PcY = Pc.at<float>(1);
    const float&  PcZ = Pc.at<float>(2);
 
    // Check positive depth
    if (PcZ < 0.0f)
        return false;
 
    // Project in image and check it is not outside
    const float invz = 1.0f / PcZ;
    const float u    = fx * PcX * invz + cx;
    const float v    = fy * PcY * invz + cy;
 
    if (u < mnMinX || u > mnMaxX)
        return false;
    if (v < mnMinY || v > mnMaxY)
        return false;
 
    // Check distance is in the scale invariance region of the WAIMapPoint
    const float   maxDistance = pMP->GetMaxDistanceInvariance();
    const float   minDistance = pMP->GetMinDistanceInvariance();
    const cv::Mat PO          = P - mOw;
    const float   dist        = (float)cv::norm(PO);
 
    if (dist < minDistance || dist > maxDistance)
        return false;
 
    // Check viewing angle
    cv::Mat Pn = pMP->GetNormal();
 
    const float viewCos = (float)(PO.dot(Pn) / dist);
 
    if (viewCos < viewingCosLimit)
        return false;
 
    // Predict scale in the image
    const int nPredictedLevel = pMP->PredictScale(dist, this);
 
    // Data used by the tracking
    pMP->mbTrackInView = true;
    pMP->mTrackProjX   = u;
    //pMP->mTrackProjXR = u - mbf*invz;
    pMP->mTrackProjY       = v;
    pMP->mnTrackScaleLevel = nPredictedLevel;
    pMP->mTrackViewCos     = viewCos;
 
    return true;
}

PosInGrid()

bool WAIFrame::PosInGrid	(	const cv::KeyPoint &	kp,
		int &	posX,
		int &	posY
	)

Definition at line 305 of file WAIFrame.cpp.

{
    posX = (int)round((kp.pt.x - mnMinX) * mfGridElementWidthInv);
    posY = (int)round((kp.pt.y - mnMinY) * mfGridElementHeightInv);
 
    //Keypoint's coordinates are undistorted, which could cause to go out of the image
    if (posX < 0 || posX >= FRAME_GRID_COLS || posY < 0 || posY >= FRAME_GRID_ROWS)
        return false;
 
    return true;
}

SetPose()

void WAIFrame::SetPose

(

cv::Mat

Tcw

)

Definition at line 178 of file WAIFrame.cpp.

{
    mTcw = Tcw.clone();
    UpdatePoseMatrices();
}

UndistortKeyPoints()

void WAIFrame::UndistortKeyPoints ( )

private

Definition at line 330 of file WAIFrame.cpp.

{
    if (mDistCoef.at<float>(0) == 0.0f)
    {
        mvKeysUn = mvKeys;
        return;
    }
 
    // Fill matrix with points
    cv::Mat mat(N, 2, CV_32F);
    for (int i = 0; i < N; i++)
    {
        mat.at<float>(i, 0) = mvKeys[i].pt.x;
        mat.at<float>(i, 1) = mvKeys[i].pt.y;
    }
 
    // Undistort points
    mat = mat.reshape(2);
    cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);
    mat = mat.reshape(1);
 
    // Fill undistorted keypoint vector
    mvKeysUn.resize(N);
    for (int i = 0; i < N; i++)
    {
        cv::KeyPoint kp = mvKeys[i];
        kp.pt.x         = mat.at<float>(i, 0);
        kp.pt.y         = mat.at<float>(i, 1);
        mvKeysUn[i]     = kp;
    }
}

UpdatePoseMatrices()

void WAIFrame::UpdatePoseMatrices

(

)

Definition at line 184 of file WAIFrame.cpp.

{
    mRcw = mTcw.rowRange(0, 3).colRange(0, 3);
    mRwc = mRcw.t();
    mtcw = mTcw.rowRange(0, 3).col(3);
    mOw  = -mRwc * mtcw;
}

Member Data Documentation

cx

float WAIFrame::cx = 0.0f

static

Definition at line 116 of file WAIFrame.h.

cy

float WAIFrame::cy = 0.0f

static

Definition at line 117 of file WAIFrame.h.

fx

float WAIFrame::fx = 0.0f

static

Definition at line 114 of file WAIFrame.h.

fy

float WAIFrame::fy = 0.0f

static

Definition at line 115 of file WAIFrame.h.

imgGray

cv::Mat WAIFrame::imgGray

Definition at line 185 of file WAIFrame.h.

invfx

float WAIFrame::invfx = 0.0f

static

Definition at line 118 of file WAIFrame.h.

invfy

float WAIFrame::invfy = 0.0f

static

Definition at line 119 of file WAIFrame.h.

mbInitialComputations

bool WAIFrame::mbInitialComputations = true

static

Definition at line 182 of file WAIFrame.h.

mBowVec

WAIBowVector WAIFrame::mBowVec

Definition at line 137 of file WAIFrame.h.

mDescriptors

cv::Mat WAIFrame::mDescriptors

Definition at line 141 of file WAIFrame.h.

mDistCoef

cv::Mat WAIFrame::mDistCoef

Definition at line 120 of file WAIFrame.h.

mFeatVec

WAIFeatVector WAIFrame::mFeatVec

Definition at line 138 of file WAIFrame.h.

mfGridElementHeightInv

float WAIFrame::mfGridElementHeightInv = 0.0f

static

Definition at line 151 of file WAIFrame.h.

mfGridElementWidthInv

float WAIFrame::mfGridElementWidthInv = 0.0f

static

Definition at line 150 of file WAIFrame.h.

mfLogScaleFactor

float WAIFrame::mfLogScaleFactor

Definition at line 170 of file WAIFrame.h.

mfScaleFactor

float WAIFrame::mfScaleFactor

Definition at line 169 of file WAIFrame.h.

mGrid

std::vector<std::size_t> WAIFrame::mGrid[64][36]

Definition at line 152 of file WAIFrame.h.

mK

cv::Mat WAIFrame::mK

Definition at line 113 of file WAIFrame.h.

mnId

long unsigned int WAIFrame::mnId = -1

Definition at line 159 of file WAIFrame.h.

mnMaxX

float WAIFrame::mnMaxX = 0.0f

static

Definition at line 178 of file WAIFrame.h.

mnMaxY

float WAIFrame::mnMaxY = 0.0f

static

Definition at line 180 of file WAIFrame.h.

mnMinX

float WAIFrame::mnMinX = 0.0f

static

Definition at line 177 of file WAIFrame.h.

mnMinY

float WAIFrame::mnMinY = 0.0f

static

Definition at line 179 of file WAIFrame.h.

mnScaleLevels

int WAIFrame::mnScaleLevels

Definition at line 168 of file WAIFrame.h.

mOw

cv::Mat WAIFrame::mOw

private

Definition at line 203 of file WAIFrame.h.

mpORBextractorLeft

KPextractor* WAIFrame::mpORBextractorLeft = NULL

Definition at line 107 of file WAIFrame.h.

mpReferenceKF

WAIKeyFrame* WAIFrame::mpReferenceKF = NULL

Definition at line 165 of file WAIFrame.h.

mRcw

cv::Mat WAIFrame::mRcw

private

Definition at line 200 of file WAIFrame.h.

mRwc

cv::Mat WAIFrame::mRwc

private

Definition at line 202 of file WAIFrame.h.

mTcw

cv::Mat WAIFrame::mTcw

Definition at line 155 of file WAIFrame.h.

mtcw

cv::Mat WAIFrame::mtcw

private

Definition at line 201 of file WAIFrame.h.

mTimeStamp

double WAIFrame::mTimeStamp

Definition at line 110 of file WAIFrame.h.

mvDepth

std::vector<float> WAIFrame::mvDepth

Definition at line 134 of file WAIFrame.h.

mvInvLevelSigma2

std::vector<float> WAIFrame::mvInvLevelSigma2

Definition at line 174 of file WAIFrame.h.

mvInvScaleFactors

std::vector<float> WAIFrame::mvInvScaleFactors

Definition at line 172 of file WAIFrame.h.

mvKeys

std::vector<cv::KeyPoint> WAIFrame::mvKeys

Definition at line 128 of file WAIFrame.h.

mvKeysUn

std::vector<cv::KeyPoint> WAIFrame::mvKeysUn

Definition at line 129 of file WAIFrame.h.

mvLevelSigma2

std::vector<float> WAIFrame::mvLevelSigma2

Definition at line 173 of file WAIFrame.h.

mVocabulary

WAIOrbVocabulary* WAIFrame::mVocabulary = NULL

Definition at line 104 of file WAIFrame.h.

mvpMapPoints

std::vector<WAIMapPoint*> WAIFrame::mvpMapPoints

Definition at line 147 of file WAIFrame.h.

mvScaleFactors

std::vector<float> WAIFrame::mvScaleFactors

Definition at line 171 of file WAIFrame.h.

mvuRight

std::vector<float> WAIFrame::mvuRight

Definition at line 133 of file WAIFrame.h.

N

int WAIFrame::N = 0

Definition at line 123 of file WAIFrame.h.

nNextId

long unsigned int WAIFrame::nNextId = 0

static

Definition at line 158 of file WAIFrame.h.

---

The documentation for this class was generated from the following files:

• WAIFrame.h
• WAIFrame.cpp