orb_slam3_details/include/Atlas.h

175 lines
4.9 KiB
C
Raw Permalink Normal View History

2020-12-01 11:58:17 +08:00
/**
* This file is part of ORB-SLAM3
*
2022-03-28 21:20:28 +08:00
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
2020-12-01 11:58:17 +08:00
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef ATLAS_H
#define ATLAS_H
#include "Map.h"
#include "MapPoint.h"
#include "KeyFrame.h"
#include "GeometricCamera.h"
#include "Pinhole.h"
#include "KannalaBrandt8.h"
#include <set>
#include <mutex>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/export.hpp>
namespace ORB_SLAM3
{
class Viewer;
class Map;
class MapPoint;
class KeyFrame;
class KeyFrameDatabase;
class Frame;
class KannalaBrandt8;
class Pinhole;
2022-03-28 21:20:28 +08:00
//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
2020-12-01 11:58:17 +08:00
class Atlas
{
2022-03-28 21:20:28 +08:00
// 1. 对boost声明友元boost就能调用Atlas的serialize了
friend class boost::serialization::access;
// 保存读取都用这个
template<class Archive>
void serialize(Archive &ar, const unsigned int version)
{
// 由于保存相机是基类,但是实际使用是派生类,所以声明一下
ar.template register_type<Pinhole>();
ar.template register_type<KannalaBrandt8>();
// Save/load a set structure, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
//ar & mspMaps;
// 基础类型不用管但是自定义的类里面要进一步写serialize函数确定保存内容
ar & mvpBackupMaps;
ar & mvpCameras;
// Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map
ar & Map::nNextId;
ar & Frame::nNextId;
ar & KeyFrame::nNextId;
ar & MapPoint::nNextId;
ar & GeometricCamera::nNextId;
ar & mnLastInitKFidMap;
}
2020-12-01 11:58:17 +08:00
public:
2022-03-28 21:20:28 +08:00
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
2020-12-01 11:58:17 +08:00
Atlas();
Atlas(int initKFid); // When its initialization the first map is created
~Atlas();
2022-03-28 21:20:28 +08:00
2020-12-01 11:58:17 +08:00
void CreateNewMap();
void ChangeMap(Map* pMap);
unsigned long int GetLastInitKFid();
void SetViewer(Viewer* pViewer);
// Method for change components in the current map
void AddKeyFrame(KeyFrame* pKF);
void AddMapPoint(MapPoint* pMP);
2022-03-28 21:20:28 +08:00
//void EraseMapPoint(MapPoint* pMP);
//void EraseKeyFrame(KeyFrame* pKF);
2020-12-01 11:58:17 +08:00
2022-03-28 21:20:28 +08:00
GeometricCamera* AddCamera(GeometricCamera* pCam);
std::vector<GeometricCamera*> GetAllCameras();
2020-12-01 11:58:17 +08:00
/* All methods without Map pointer work on current map */
void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
void InformNewBigChange();
int GetLastBigChangeIdx();
long unsigned int MapPointsInMap();
long unsigned KeyFramesInMap();
// Method for get data in current map
std::vector<KeyFrame*> GetAllKeyFrames();
std::vector<MapPoint*> GetAllMapPoints();
std::vector<MapPoint*> GetReferenceMapPoints();
vector<Map*> GetAllMaps();
int CountMaps();
void clearMap();
void clearAtlas();
Map* GetCurrentMap();
void SetMapBad(Map* pMap);
void RemoveBadMaps();
bool isInertial();
void SetInertialSensor();
void SetImuInitialized();
bool isImuInitialized();
2022-03-28 21:20:28 +08:00
// Function for garantee the correction of serialization of this object
void PreSave();
void PostLoad();
map<long unsigned int, KeyFrame*> GetAtlasKeyframes();
2020-12-01 11:58:17 +08:00
void SetKeyFrameDababase(KeyFrameDatabase* pKFDB);
KeyFrameDatabase* GetKeyFrameDatabase();
void SetORBVocabulary(ORBVocabulary* pORBVoc);
ORBVocabulary* GetORBVocabulary();
long unsigned int GetNumLivedKF();
long unsigned int GetNumLivedMP();
protected:
2022-03-28 21:20:28 +08:00
std::set<Map*> mspMaps;
2020-12-01 11:58:17 +08:00
std::set<Map*> mspBadMaps;
2022-03-28 21:20:28 +08:00
// Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer
std::vector<Map*> mvpBackupMaps;
2020-12-01 11:58:17 +08:00
Map* mpCurrentMap;
std::vector<GeometricCamera*> mvpCameras;
unsigned long int mnLastInitKFidMap;
Viewer* mpViewer;
bool mHasViewer;
// Class references for the map reconstruction from the save file
KeyFrameDatabase* mpKeyFrameDB;
ORBVocabulary* mpORBVocabulary;
2022-03-28 21:20:28 +08:00
// Mutex
std::mutex mMutexAtlas;
2020-12-01 11:58:17 +08:00
}; // class Atlas
2024-07-04 17:24:09 +08:00
} // namespace orb_slam3
2020-12-01 11:58:17 +08:00
#endif // ATLAS_H