104 lines
3.7 KiB
C
104 lines
3.7 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 MAPOBJECT_H
|
||
|
|
#define MAPOBJECT_H
|
||
|
|
|
||
|
|
#include "KeyFrame.h"
|
||
|
|
#include "Frame.h"
|
||
|
|
#include "Map.h"
|
||
|
|
#include <map>
|
||
|
|
#include <random>
|
||
|
|
|
||
|
|
#include <Eigen/Core>
|
||
|
|
#include <Eigen/Geometry>
|
||
|
|
#include <Eigen/Dense>
|
||
|
|
#include <pcl/point_types.h>
|
||
|
|
#include <pcl/point_cloud.h>
|
||
|
|
#include <opencv2/core/core.hpp>
|
||
|
|
#include <opencv2/opencv.hpp>
|
||
|
|
#include <mutex>
|
||
|
|
|
||
|
|
namespace ORB_SLAM2
|
||
|
|
{
|
||
|
|
|
||
|
|
class KeyFrame;
|
||
|
|
class Map;
|
||
|
|
class Frame;
|
||
|
|
|
||
|
|
class MapObject
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
MapObject(cv::Mat &depth, std::vector<double> &vlabel, float width, cv::Mat &Twc, cv::Mat K, KeyFrame *pRefKF, Map *pMap);
|
||
|
|
MapObject(cv::Mat &depth, vector<double> &vlabel, float width, cv::Mat &Twc, cv::Mat K, Frame *pFrame, Map *pMap);
|
||
|
|
|
||
|
|
void AddObservation(KeyFrame *pKF, size_t idx); // 添加关键帧的观测
|
||
|
|
MapObject *GetReplaced();
|
||
|
|
void IncreaseVisible(int n = 1);
|
||
|
|
void IncreaseFound(int n = 1); // 能找到的
|
||
|
|
|
||
|
|
int Observations(); // 获取当前物体的被观测次数
|
||
|
|
|
||
|
|
float GetFoundRatio_object();
|
||
|
|
void SetBadFlag();
|
||
|
|
bool isBad(); // 是否为坏点
|
||
|
|
bool IsInKeyFrame(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);
|
||
|
|
void SetColor();
|
||
|
|
|
||
|
|
public:
|
||
|
|
pcl::PointCloud<pcl::PointXYZ> mcloud; // 点云
|
||
|
|
Eigen::Vector3d mcenter; // 质心,非齐次坐标
|
||
|
|
cv::Mat mbbox; // x1,y1,z1,x2,y2,z2,cls 世界坐标系下的坐标
|
||
|
|
float mfWidth; // 物体宽度
|
||
|
|
double mr; // 自定义物体颜色
|
||
|
|
double mg;
|
||
|
|
double mb;
|
||
|
|
|
||
|
|
bool mstate;
|
||
|
|
bool mbTrackInView;
|
||
|
|
long unsigned int mnLastFrameSeen;
|
||
|
|
const long int mnFirstKFid; ///< 创建该MapObject的关键帧ID
|
||
|
|
/// Tracking counters
|
||
|
|
int mnVisible;
|
||
|
|
int mnFound;
|
||
|
|
|
||
|
|
long unsigned int mnTrackReferenceForFrame; // TrackLocalMap - UpdateLocalObjects 中防止将MapObjects重复添加至mvpLocalMapObjects的标记
|
||
|
|
|
||
|
|
int nObs; // 被观测到的次数
|
||
|
|
long unsigned int mnId; ///< Global ID for MapObject
|
||
|
|
static long unsigned int nNextId;
|
||
|
|
bool mbBad; // 是否是坏点
|
||
|
|
Map *mpMap; /// 所属的地图
|
||
|
|
|
||
|
|
protected:
|
||
|
|
std::map<KeyFrame *, size_t> mObservations; ///< 观测到该MapObject的KF和该MapObject在KF中的索引
|
||
|
|
std::mutex mMutexObject; // 对当前物体模型进行操作的互斥量（如添加观测关系的时候）
|
||
|
|
MapObject *mpReplaced; // 替换当前物体的新物体
|
||
|
|
};
|
||
|
|
|
||
|
|
} // namespace ORB_SLAM
|
||
|
|
|
||
|
|
#endif // MAPOBJECT_H
|