262 lines
8.5 KiB
C++
262 lines
8.5 KiB
C++
/**
|
||
* This file is part of ORB-SLAM2.
|
||
*
|
||
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
|
||
* For more information see <https://github.com/raulmur/ORB_SLAM2>
|
||
*
|
||
* ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
#ifndef TRACKING_H
|
||
#define TRACKING_H
|
||
|
||
#include <opencv2/core/core.hpp>
|
||
#include <opencv2/features2d/features2d.hpp>
|
||
|
||
#include "Viewer.h"
|
||
#include "FrameDrawer.h"
|
||
#include "Map.h"
|
||
#include "LocalMapping.h"
|
||
#include "LoopClosing.h"
|
||
#include "Frame.h"
|
||
#include "ORBVocabulary.h"
|
||
#include "KeyFrameDatabase.h"
|
||
#include "ORBextractor.h"
|
||
#include "Initializer.h"
|
||
#include "MapDrawer.h"
|
||
#include "System.h"
|
||
|
||
#include <mutex>
|
||
|
||
namespace ORB_SLAM2
|
||
{
|
||
|
||
class Viewer;
|
||
class FrameDrawer;
|
||
class Map;
|
||
class LocalMapping;
|
||
class LoopClosing;
|
||
class System;
|
||
|
||
class Tracking
|
||
{
|
||
|
||
public:
|
||
Tracking(System *pSys, ORBVocabulary *pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap,
|
||
KeyFrameDatabase *pKFDB, const string &strSettingPath, const int sensor);
|
||
|
||
// Preprocess the input and call Track(). Extract features and performs stereo matching.
|
||
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp);
|
||
cv::Mat GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double ×tamp, const vector<vector<double>> &vLabel);
|
||
cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp);
|
||
|
||
void SetLocalMapper(LocalMapping *pLocalMapper);
|
||
void SetLoopClosing(LoopClosing *pLoopClosing);
|
||
void SetViewer(Viewer *pViewer);
|
||
|
||
// Load new settings
|
||
// The focal lenght should be similar or scale prediction will fail when projecting points
|
||
// TODO: Modify MapPoint::PredictScale to take into account focal lenght
|
||
void ChangeCalibration(const string &strSettingPath);
|
||
|
||
// Use this function if you have deactivated local mapping and you only want to localize the camera.
|
||
void InformOnlyTracking(const bool &flag);
|
||
|
||
// 获得相对位姿 --by钟
|
||
cv::Mat GetVelocity() { return this->mVelocity.clone(); }
|
||
|
||
cv::Mat Get_T_Inverse(cv::Mat &Tcw);
|
||
// 是否需要添加进物体模型库
|
||
bool NeedAddObject(MapObject *pObj, int cls, int &min_dis_idx);
|
||
|
||
public:
|
||
// Tracking states
|
||
enum eTrackingState
|
||
{
|
||
SYSTEM_NOT_READY = -1,
|
||
NO_IMAGES_YET = 0,
|
||
NOT_INITIALIZED = 1,
|
||
OK = 2,
|
||
LOST = 3
|
||
};
|
||
|
||
eTrackingState mState;
|
||
eTrackingState mLastProcessedState;
|
||
|
||
// Input sensor
|
||
int mSensor;
|
||
|
||
// Current Frame
|
||
Frame mCurrentFrame;
|
||
cv::Mat mImGray;
|
||
cv::Mat mImRGB;
|
||
|
||
// Initialization Variables (Monocular)
|
||
std::vector<int> mvIniLastMatches;
|
||
std::vector<int> mvIniMatches;
|
||
std::vector<cv::Point2f> mvbPrevMatched;
|
||
std::vector<cv::Point3f> mvIniP3D;
|
||
Frame mInitialFrame;
|
||
|
||
// Lists used to recover the full camera trajectory at the end of the execution.
|
||
// Basically we store the reference keyframe for each frame and its relative transformation
|
||
list<cv::Mat> mlRelativeFramePoses;
|
||
list<KeyFrame *> mlpReferences;
|
||
list<double> mlFrameTimes;
|
||
list<bool> mlbLost;
|
||
|
||
// True if local mapping is deactivated and we are performing only localization
|
||
bool mbOnlyTracking;
|
||
|
||
void Reset();
|
||
|
||
// 目标检测数据
|
||
vector<vector<double>> mvvLabel; // cls,x,y,w,h
|
||
|
||
// 物体模型库
|
||
vector<vector<MapObject *>> mvvpObjModel;
|
||
|
||
// 要添加进BA质心对约束
|
||
vector<vector<Eigen::Vector3d>> mvvCenters;
|
||
|
||
// 物体宽度参数
|
||
vector<float> mvObjWidth;
|
||
|
||
// 目标框生成的物体点云
|
||
vector<std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr>> mvCurrentObjCloud;
|
||
vector<std::map<int, Eigen::Vector3d>> mvCurrentCenters; // map<id,centers>
|
||
|
||
// 记录目标框和模型库的匹配关系
|
||
vector<std::map<int, int>> mvObjMatches; // <目标框id，模型库id>
|
||
|
||
protected:
|
||
// Main tracking function. It is independent of the input sensor.
|
||
void Track();
|
||
|
||
// Map initialization for stereo and RGB-D
|
||
void StereoInitialization();
|
||
|
||
// Map initialization for monocular
|
||
void MonocularInitialization();
|
||
void CreateInitialMapMonocular();
|
||
|
||
void CheckReplacedInLastFrame();
|
||
void CheckReplacedInLastFrameWithObject();
|
||
bool TrackReferenceKeyFrame();
|
||
|
||
void UpdateLastFrame();
|
||
void UpdateLastFrameWithObj();
|
||
|
||
bool TrackWithMotionModel();
|
||
|
||
bool Relocalization();
|
||
|
||
void UpdateLocalMap();
|
||
void UpdateLocalPoints();
|
||
void UpdateLocalKeyFrames();
|
||
void UpdateLocalObjects(); // 更新局部物体
|
||
|
||
bool TrackLocalMap();
|
||
void SearchLocalPoints();
|
||
void SearchLocalObjects(); // 对 Local Objects 进行跟踪
|
||
|
||
bool NeedNewKeyFrame();
|
||
void CreateNewKeyFrame();
|
||
|
||
void CreateObjMatches();
|
||
void CreateObjMatchesLast2Current(Frame &CurrentFrame, Frame &LastFrame);
|
||
void CreateObjMatchesReKF2Current(Frame &CurrentFrame, KeyFrame *pKF);
|
||
Eigen::Isometry3d Mat2Eigen(cv::Mat &Twc);
|
||
bool dbscan(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, // DBSCAN聚类
|
||
std::vector<std::vector<int>> &clusters_index,
|
||
const double &r, const int &size, int &idx);
|
||
// In case of performing only localization, this flag is true when there are no matches to
|
||
// points in the map. Still tracking will continue if there are enough matches with temporal points.
|
||
// In that case we are doing visual odometry. The system will try to do relocalization to recover
|
||
// "zero-drift" localization to the map.
|
||
bool mbVO;
|
||
|
||
// Other Thread Pointers
|
||
LocalMapping *mpLocalMapper;
|
||
LoopClosing *mpLoopClosing;
|
||
|
||
// ORB
|
||
ORBextractor *mpORBextractorLeft, *mpORBextractorRight;
|
||
ORBextractor *mpIniORBextractor;
|
||
|
||
// BoW
|
||
ORBVocabulary *mpORBVocabulary;
|
||
KeyFrameDatabase *mpKeyFrameDB;
|
||
|
||
// Initalization (only for monocular)
|
||
Initializer *mpInitializer;
|
||
|
||
// Local Map
|
||
KeyFrame *mpReferenceKF;
|
||
std::vector<KeyFrame *> mvpLocalKeyFrames;
|
||
std::vector<MapPoint *> mvpLocalMapPoints;
|
||
std::vector<MapObject *> mvpLocalMapObjects;
|
||
|
||
// System
|
||
System *mpSystem;
|
||
|
||
// Drawers
|
||
Viewer *mpViewer;
|
||
FrameDrawer *mpFrameDrawer;
|
||
MapDrawer *mpMapDrawer;
|
||
|
||
// Map
|
||
Map *mpMap;
|
||
|
||
// Calibration matrix
|
||
cv::Mat mK;
|
||
cv::Mat mDistCoef;
|
||
float mbf;
|
||
|
||
// New KeyFrame rules (according to fps)
|
||
int mMinFrames;
|
||
int mMaxFrames;
|
||
|
||
// Threshold close/far points
|
||
// Points seen as close by the stereo/RGBD sensor are considered reliable
|
||
// and inserted from just one frame. Far points requiere a match in two keyframes.
|
||
float mThDepth;
|
||
|
||
// For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
|
||
float mDepthMapFactor;
|
||
|
||
// Current matches in frame
|
||
int mnMatchesInliers;
|
||
int mnObjMatchesInliers;
|
||
|
||
// Last Frame, KeyFrame and Relocalisation Info
|
||
KeyFrame *mpLastKeyFrame;
|
||
Frame mLastFrame;
|
||
unsigned int mnLastKeyFrameId;
|
||
unsigned int mnLastRelocFrameId;
|
||
|
||
// Motion Model
|
||
cv::Mat mVelocity;
|
||
|
||
// Color order (true RGB, false BGR, ignored if grayscale)
|
||
bool mbRGB;
|
||
|
||
list<MapPoint *> mlpTemporalPoints; // 临时的地图点,用于提高双目和RGBD摄像头的帧间效果,用完之后就扔了
|
||
list<MapObject *> mlpTemporalObjects;
|
||
};
|
||
|
||
} // namespace ORB_SLAM
|
||
|
||
#endif // TRACKING_H
|