WAIMap Class Reference

#include <WAIMap.h>

Public Types
enum	TransformType {   ROT_X = 0 , ROT_Y , ROT_Z , TRANS_X , TRANS_Y , TRANS_Z ,   SCALE }

Public Member Functions
	WAIMap (WAIKeyFrameDB *kfDB)
	~WAIMap ()
void	AddKeyFrame (WAIKeyFrame *pKF)
void	AddMapPoint (WAIMapPoint *pMP)
void	EraseMapPoint (WAIMapPoint *pMP)
void	EraseKeyFrame (WAIKeyFrame *pKF)
void	SetReferenceMapPoints (const std::vector< WAIMapPoint * > &vpMPs)
void	InformNewBigChange ()
int	GetLastBigChangeIdx ()
std::vector< WAIKeyFrame * >	GetAllKeyFrames ()
std::vector< WAIMapPoint * >	GetAllMapPoints ()
WAIKeyFrameDB *	GetKeyFrameDB ()
long unsigned int	MapPointsInMap ()
long unsigned int	KeyFramesInMap ()
long unsigned int	GetMaxKFid ()
float	GetSize ()
void	clear ()
void	transform (cv::Mat transform)
void	rotate (float degVal, int type)
void	translate (float value, int type)
void	scale (float value)
void	applyTransformation (double value, TransformType type)
cv::Mat	buildTransMat (float &val, int type)
cv::Mat	buildRotMat (float &valDeg, int type)
size_t	getSizeOf ()
bool	isKeyFrameInMap (WAIKeyFrame *pKF)
void	incNumLoopClosings ()
void	setNumLoopClosings (int n)
int	getNumLoopClosings ()

Public Attributes
vector< WAIKeyFrame * >	mvpKeyFrameOrigins
std::mutex	mMutexMapUpdate
std::mutex	mMutexPointCreation

Protected Attributes
std::set< WAIMapPoint * >	mspMapPoints
std::set< WAIKeyFrame * >	mspKeyFrames
std::vector< WAIKeyFrame * >	_deletedKeyFrames
WAIKeyFrameDB *	mKfDB {nullptr}
std::vector< WAIMapPoint * >	mvpReferenceMapPoints
long unsigned int	mnMaxKFid
int	mnBigChangeIdx
std::mutex	mMutexMap
std::mutex	_mutexLoopClosings
int	_numberOfLoopClosings = 0
int	_numOfKeyframes

Detailed Description

Definition at line 51 of file WAIMap.h.

Member Enumeration Documentation

TransformType

enum WAIMap::TransformType

Enumerator
ROT_X
ROT_Y
ROT_Z
TRANS_X
TRANS_Y
TRANS_Z
SCALE

Definition at line 54 of file WAIMap.h.

    {
        ROT_X = 0,
        ROT_Y,
        ROT_Z,
        TRANS_X,
        TRANS_Y,
        TRANS_Z,
        SCALE
    };

Constructor & Destructor Documentation

WAIMap()

WAIMap::WAIMap

(

WAIKeyFrameDB *

kfDB

)

Definition at line 36 of file WAIMap.cpp.

  : mnMaxKFid(0), mnBigChangeIdx(0)
{
    mKfDB = kfDB;
}

~WAIMap()

WAIMap::~WAIMap

(

)

Definition at line 42 of file WAIMap.cpp.

{
    clear();
    if (mKfDB)
        delete mKfDB;
}

Member Function Documentation

AddKeyFrame()

void WAIMap::AddKeyFrame

(

WAIKeyFrame *

pKF

)

Definition at line 49 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    mspKeyFrames.insert(pKF);
    if (pKF->mnId > mnMaxKFid)
        mnMaxKFid = pKF->mnId;
    //mKfDB->add(pKF);
}

AddMapPoint()

void WAIMap::AddMapPoint

(

WAIMapPoint *

pMP

)

Definition at line 58 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    mspMapPoints.insert(pMP);
}

applyTransformation()

void WAIMap::applyTransformation	(	double	value,
		TransformType	type
	)

Definition at line 320 of file WAIMap.cpp.

{
    //apply rotation, translation and scale to Keyframe and MapPoint poses
    cout << "apply transform with value: " << value << endl;
    switch (type)
    {
        case ROT_X:
            //build different transformation matrices for x,y and z rotation
            rotate((float)value, 0);
            break;
        case ROT_Y:
            rotate((float)value, 1);
            break;
        case ROT_Z:
            rotate((float)value, 2);
            break;
        case TRANS_X:
            translate((float)value, 0);
            break;
        case TRANS_Y:
            translate((float)value, 1);
            break;
        case TRANS_Z:
            translate((float)value, 2);
            break;
        case SCALE:
            scale((float)value);
            break;
    }
 
    //compute resulting values for map points
    for (auto& mp : mspMapPoints)
    {
        //mean viewing direction and depth
        mp->UpdateNormalAndDepth();
        mp->ComputeDistinctiveDescriptors();
    }
 
#if 0
    //update scene objects
    //exchange all Keyframes (also change name)
    if (_mapNode)
    {
        _mapNode->updateAll(*this);
    }
    else
    {
        // TODO(jan): handle errors
        //SL_WARN_MSG("WAIMap: applyTransformation: WAIMapNode is NULL! Cannot update visualization!\n");
    }
#endif
}

buildRotMat()

Mat WAIMap::buildRotMat	(	float &	valDeg,
		int	type
	)

Definition at line 398 of file WAIMap.cpp.

{
    Mat rot = Mat::ones(4, 4, CV_32F);
 
    switch (type)
    {
        case 0:
            // Calculate rotation about x axis
            rot = (Mat_<float>(4, 4) << 1, 0, 0, 0, 0, cos(valDeg), -sin(valDeg), 0, 0, sin(valDeg), cos(valDeg), 0, 0, 0, 0, 1);
            break;
 
        case 1:
            // Calculate rotation about y axis
            rot = (Mat_<float>(4, 4) << cos(valDeg), 0, sin(valDeg), 0, 0, 1, 0, 0, -sin(valDeg), 0, cos(valDeg), 0, 0, 0, 0, 1);
            //invert direction for Y
            rot = rot.inv();
            break;
 
        case 2:
            // Calculate rotation about z axis
            rot = (Mat_<float>(4, 4) << cos(valDeg), -sin(valDeg), 0, 0, sin(valDeg), cos(valDeg), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
            //invert direction for Z
            rot = rot.inv();
            break;
    }
 
    return rot;
}

buildTransMat()

Mat WAIMap::buildTransMat	(	float &	val,
		int	type
	)

!turn sign of y coordinate

!turn sign of z coordinate

Definition at line 374 of file WAIMap.cpp.

{
    Mat trans = cv::Mat::zeros(3, 1, CV_32F);
    switch (type)
    {
        case 0:
            trans.at<float>(0, 0) = val;
            break;
 
        case 1:
            //!!turn sign of y coordinate
            trans.at<float>(1, 0) = -val;
            break;
 
        case 2:
            //!!turn sign of z coordinate
            trans.at<float>(2, 0) = -val;
            break;
    }
 
    return trans;
}

clear()

void WAIMap::clear

(

)

Definition at line 158 of file WAIMap.cpp.

{
    for (auto* pt : mspMapPoints)
    {
        if (pt)
            delete pt;
    }
    for (auto* kf : mspKeyFrames)
    {
        if (kf)
            delete kf;
    }
    mspMapPoints.clear();
    mspKeyFrames.clear();
    mnMaxKFid = 0;
    mvpReferenceMapPoints.clear();
    mvpKeyFrameOrigins.clear();
    setNumLoopClosings(0);
    mKfDB->clear();
 
#if 0
    for (WAIKeyFrame* kf : _deletedKeyFrames)
    {
        delete kf;
    }
 
    _deletedKeyFrames.clear();
#endif
 
    WAIKeyFrame::nNextId = 0;
    WAIFrame::nNextId    = 0;
}

EraseKeyFrame()

void WAIMap::EraseKeyFrame

(

WAIKeyFrame *

pKF

)

Definition at line 74 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    mspKeyFrames.erase(pKF);
    mKfDB->erase(pKF);
 
    //_deletedKeyFrames.push_back(pKF);
 
    // TODO: This only erase the pointer.
    // Delete the MapPoint
}

EraseMapPoint()

void WAIMap::EraseMapPoint

(

WAIMapPoint *

pMP

)

Definition at line 64 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    mspMapPoints.erase(pMP);
 
    // TODO: This only erase the pointer.
    // Delete the MapPoint
    //delete pMP;
}

GetAllKeyFrames()

std::vector< WAIKeyFrame * > WAIMap::GetAllKeyFrames

(

)

Definition at line 104 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    return vector<WAIKeyFrame*>(mspKeyFrames.begin(), mspKeyFrames.end());
}

GetAllMapPoints()

vector< WAIMapPoint * > WAIMap::GetAllMapPoints

(

)

Definition at line 110 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    return vector<WAIMapPoint*>(mspMapPoints.begin(), mspMapPoints.end());
}

GetKeyFrameDB()

WAIKeyFrameDB* WAIMap::GetKeyFrameDB ( )

inline

Definition at line 78 of file WAIMap.h.

{ return mKfDB; }

GetLastBigChangeIdx()

int WAIMap::GetLastBigChangeIdx

(

)

Definition at line 98 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    return mnBigChangeIdx;
}

GetMaxKFid()

long unsigned int WAIMap::GetMaxKFid

(

)

Definition at line 128 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    return mnMaxKFid;
}

getNumLoopClosings()

int WAIMap::getNumLoopClosings

(

)

Definition at line 475 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(_mutexLoopClosings);
    return _numberOfLoopClosings;
}

GetSize()

float WAIMap::GetSize

(

)

Definition at line 134 of file WAIMap.cpp.

{
    std::vector<WAIMapPoint*> mpv = GetAllMapPoints();
    WAI::V3                   a   = mpv[0]->worldPosVec();
    WAI::V3                   b   = a;
 
    for (WAIMapPoint* mp : mpv)
    {
        WAI::V3 v = mp->worldPosVec();
        a.x       = fmax(v.x, a.x);
        a.y       = fmax(v.y, a.y);
        a.z       = fmax(v.z, a.z);
 
        b.x = fmin(v.x, b.x);
        b.y = fmin(v.y, b.y);
        b.z = fmin(v.z, b.z);
    }
 
    a = WAI::v3(a.x - b.x, a.y - b.y, a.z - b.z);
 
    return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
}

getSizeOf()

size_t WAIMap::getSizeOf

(

)

Definition at line 427 of file WAIMap.cpp.

{
    size_t size = 0;
 
    //size of map points
    std::size_t sizeOfMapPoints = 0;
    for (auto mp : mspMapPoints)
    {
        sizeOfMapPoints += mp->getSizeOf();
    }
 
    //size of key frames
    std::size_t sizeOfKeyFrames = 0;
    for (auto kf : mspKeyFrames)
    {
        sizeOfKeyFrames += kf->getSizeOf();
    }
 
    //size of map
    size += sizeof(*this);
    size += sizeOfMapPoints;
    size += sizeOfKeyFrames;
 
    //cout << "all map points in MB: " << (double)sizeOfMapPoints / 1048576L << endl;
    //cout << "all keyframes in MB: " << (double)sizeOfKeyFrames / 1048576L << endl;
    //cout << "total map size in MB: " << (double)size / 1048576L << endl;
 
    return size;
}

incNumLoopClosings()

void WAIMap::incNumLoopClosings

(

)

Definition at line 463 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(_mutexLoopClosings);
    _numberOfLoopClosings++;
}

InformNewBigChange()

void WAIMap::InformNewBigChange

(

)

Definition at line 92 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    mnBigChangeIdx++;
}

isKeyFrameInMap()

bool WAIMap::isKeyFrameInMap

(

WAIKeyFrame *

pKF

)

Definition at line 457 of file WAIMap.cpp.

{
    bool result = (mspKeyFrames.find(pKF) != mspKeyFrames.end());
    return result;
}

KeyFramesInMap()

long unsigned int WAIMap::KeyFramesInMap

(

)

Definition at line 116 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    return (unsigned int)mspKeyFrames.size();
}

MapPointsInMap()

long unsigned int WAIMap::MapPointsInMap

(

)

Definition at line 122 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    return (unsigned int)mspMapPoints.size();
}

rotate()

void WAIMap::rotate	(	float	degVal,
		int	type
	)

Definition at line 242 of file WAIMap.cpp.

{
    //transform to degree
    float value = degVal * Utils::DEG2RAD;
 
    Mat rot = buildRotMat(value, type);
    cout << "rot: " << rot << endl;
 
    //rotate keyframes
    Mat Twc;
    for (auto& kf : mspKeyFrames)
    {
        //get and rotate
        Twc = kf->GetPose().inv();
        Twc = rot * Twc;
        //set back
        kf->SetPose(Twc.inv());
    }
 
    //rotate keypoints
    Mat Pw;
    Mat rot33 = rot.rowRange(0, 3).colRange(0, 3);
    for (auto& pt : mspMapPoints)
    {
        Pw = rot33 * pt->GetWorldPos();
        pt->SetWorldPos(rot33 * pt->GetWorldPos());
    }
}

scale()

void WAIMap::scale

(

float

value

)

Definition at line 295 of file WAIMap.cpp.

{
    for (auto& kf : mspKeyFrames)
    {
        //get and translate
        cv::Mat Tcw = kf->GetPose();
        std::cout << "Tcw before: " << Tcw << std::endl;
        Tcw.rowRange(0, 3).col(3) *= value;
        std::cout << "Tcw after: " << Tcw << std::endl;
 
        //make scale matrix
        //cv::Mat scale = cv::Mat::eye(4, 4, Tcw.type());
        //scale *= value
 
        //set back
        kf->SetPose(Tcw);
    }
 
    //rotate keypoints
    for (auto& pt : mspMapPoints)
    {
        pt->SetWorldPos(value * pt->GetWorldPos());
    }
}

setNumLoopClosings()

void WAIMap::setNumLoopClosings

(

int

n

)

Definition at line 469 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(_mutexLoopClosings);
    _numberOfLoopClosings = n;
}

SetReferenceMapPoints()

void WAIMap::SetReferenceMapPoints

(

const std::vector< WAIMapPoint * > &

vpMPs

)

Definition at line 86 of file WAIMap.cpp.

{
    unique_lock<mutex> lock(mMutexMap);
    mvpReferenceMapPoints = vpMPs;
}

transform()

void WAIMap::transform

(

cv::Mat

transform

)

Definition at line 192 of file WAIMap.cpp.

{
    cv::Mat t;
    cv::transpose(transform, t);
    cv::Mat s2 = t * transform;
    float   sx, sy, sz;
    sx = sqrt(s2.at<float>(0, 0));
    sy = sqrt(s2.at<float>(1, 1));
    sz = sqrt(s2.at<float>(2, 2));
 
    t = transform.clone();
    t.rowRange(0, 3).col(0) *= 1.0f / sx;
    t.rowRange(0, 3).col(1) *= 1.0f / sy;
    t.rowRange(0, 3).col(2) *= 1.0f / sz;
 
    Mat Twc;
    Mat Tcw;
    for (auto& kf : mspKeyFrames)
    {
        //get and rotate
        Tcw = kf->GetPose();
        Tcw.at<float>(0, 3) *= sx;
        Tcw.at<float>(1, 3) *= sy;
        Tcw.at<float>(2, 3) *= sz;
        Twc = Tcw.inv();
        Twc = t * Twc;
        //set back
        kf->SetPose(Twc.inv());
    }
 
    int i = 0;
    //transform keypoints
    for (auto& pt : mspMapPoints)
    {
        cv::Mat p  = (cv::Mat_<float>(4, 1) << 0, 0, 0, 1.0f);
        cv::Mat wp = pt->GetWorldPos();
        wp.copyTo(p.rowRange(0, 3));
        p = transform * p;
        p.rowRange(0, 3).copyTo(wp);
        pt->SetWorldPos(wp);
    }
 
    for (auto& mp : mspMapPoints)
    {
        //mean viewing direction and depth
        mp->UpdateNormalAndDepth();
        mp->ComputeDistinctiveDescriptors();
    }
}

translate()

void WAIMap::translate	(	float	value,
		int	type
	)

Definition at line 271 of file WAIMap.cpp.

{
    Mat trans = buildTransMat(value, type);
 
    cout << "trans: " << trans << endl;
 
    //rotate keyframes
    Mat Twc;
    for (auto& kf : mspKeyFrames)
    {
        //get and translate
        cv::Mat Twc = kf->GetPose().inv();
        Twc.rowRange(0, 3).col(3) += trans;
        //set back
        kf->SetPose(Twc.inv());
    }
 
    //rotate keypoints
    for (auto& pt : mspMapPoints)
    {
        pt->SetWorldPos(trans + pt->GetWorldPos());
    }
}

Member Data Documentation

_deletedKeyFrames

std::vector<WAIKeyFrame*> WAIMap::_deletedKeyFrames

protected

Definition at line 115 of file WAIMap.h.

_mutexLoopClosings

std::mutex WAIMap::_mutexLoopClosings

protected

Definition at line 127 of file WAIMap.h.

_numberOfLoopClosings

int WAIMap::_numberOfLoopClosings = 0

protected

Definition at line 128 of file WAIMap.h.

_numOfKeyframes

int WAIMap::_numOfKeyframes

protected

Definition at line 129 of file WAIMap.h.

mKfDB

WAIKeyFrameDB* WAIMap::mKfDB {nullptr}

protected

Definition at line 116 of file WAIMap.h.

mMutexMap

std::mutex WAIMap::mMutexMap

protected

Definition at line 125 of file WAIMap.h.

mMutexMapUpdate

std::mutex WAIMap::mMutexMapUpdate

Definition at line 90 of file WAIMap.h.

mMutexPointCreation

std::mutex WAIMap::mMutexPointCreation

Definition at line 93 of file WAIMap.h.

mnBigChangeIdx

int WAIMap::mnBigChangeIdx

protected

Definition at line 123 of file WAIMap.h.

mnMaxKFid

long unsigned int WAIMap::mnMaxKFid

protected

Definition at line 120 of file WAIMap.h.

mspKeyFrames

std::set<WAIKeyFrame*> WAIMap::mspKeyFrames

protected

Definition at line 114 of file WAIMap.h.

mspMapPoints

std::set<WAIMapPoint*> WAIMap::mspMapPoints

protected

Definition at line 113 of file WAIMap.h.

mvpKeyFrameOrigins

vector<WAIKeyFrame*> WAIMap::mvpKeyFrameOrigins

Definition at line 88 of file WAIMap.h.

mvpReferenceMapPoints

std::vector<WAIMapPoint*> WAIMap::mvpReferenceMapPoints

protected

Definition at line 118 of file WAIMap.h.

---

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

• WAIMap.h
• WAIMap.cpp