85 lines
2.9 KiB
C
85 lines
2.9 KiB
C
|
#ifndef SEMANTIC_MAP_H
|
|||
|
|
#define SEMANTIC_MAP_H
|
|||
|
|
|
|||
|
|
#include <iostream>
|
|||
|
|
#include <Eigen/Geometry>
|
|||
|
|
#include <opencv2/opencv.hpp>
|
|||
|
|
#include <opencv2/core/core.hpp>
|
|||
|
|
#include <opencv2/highgui/highgui.hpp>
|
|||
|
|
#include <pcl/point_types.h>
|
|||
|
|
#include <pcl/io/pcd_io.h>
|
|||
|
|
#include <pcl/filters/voxel_grid.h>
|
|||
|
|
#include <pcl/visualization/pcl_visualizer.h>
|
|||
|
|
#include <pcl/filters/statistical_outlier_removal.h>
|
|||
|
|
#include <pcl/visualization/pcl_visualizer.h>
|
|||
|
|
#include <pcl/visualization/cloud_viewer.h>
|
|||
|
|
|
|||
|
|
#include "KeyFrame.h"
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
using namespace cv;
|
|||
|
|
// 定义点云使用的格式：这里用的是XYZRGB
|
|||
|
|
typedef pcl::PointXYZ PointT;
|
|||
|
|
typedef pcl::PointCloud<PointT> PointCloud;
|
|||
|
|
|
|||
|
|
namespace ORB_SLAM2
|
|||
|
|
{
|
|||
|
|
class KeyFrame;
|
|||
|
|
|
|||
|
|
class Semantic_Map
|
|||
|
|
{
|
|||
|
|
public:
|
|||
|
|
Semantic_Map(const string &strSettingPath);
|
|||
|
|
void LoadLabel(const string &strLabelFilename, vector<vector<double>> &vvLabel);
|
|||
|
|
void get_bbx(const vector<double> &label1, std::vector<cv::Point> &pts, double scale);
|
|||
|
|
void Run();
|
|||
|
|
void InsertKeyFrame(KeyFrame *pKF);
|
|||
|
|
bool CheckNewKeyFrames();
|
|||
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr GenratePointCloudMap(KeyFrame *mpCurrentKF, cv::String cloudAddress, cv::String imgadress);
|
|||
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr GenratePointCloudMap(KeyFrame *mpCurrentKF, cv::String cloudAddress, cv::String imgadress, std::vector<std::vector<double>> vvlabel);
|
|||
|
|
void SetPose(Eigen::Isometry3d &T, cv::Mat &Twc);
|
|||
|
|
// void SetCurrentKeyFrame(KeyFrame *pkf);
|
|||
|
|
void Transform3DPointFromLidarToImage02KITTI(const cv::Mat &x3Dl, cv::Mat &x3Dc, cv::Point2i &point2D, float &d);
|
|||
|
|
/**请求终止当前线程 */
|
|||
|
|
void RequestFinish();
|
|||
|
|
/** 当前线程的run函数是否已经终止 */
|
|||
|
|
bool isFinished();
|
|||
|
|
bool CheckFinish();
|
|||
|
|
/** 设置当前线程已经真正地结束了,由本线程run函数调用 */
|
|||
|
|
void SetFinish();
|
|||
|
|
bool Stop();
|
|||
|
|
bool isStopped();
|
|||
|
|
|
|||
|
|
public:
|
|||
|
|
// pcl::PointCloud<pcl::PointXYZI>::Ptr mpPointCloud;
|
|||
|
|
std::vector<cv::String> mvpointcloud_adress;
|
|||
|
|
std::vector<cv::String> mvRGBIMG_adress;
|
|||
|
|
std::vector<std::vector<std::vector<double>>> mvvvLabel;
|
|||
|
|
vector<vector<double>> mvDynamic_label;
|
|||
|
|
cv::Mat mRGB;
|
|||
|
|
cv::Mat mDepth;
|
|||
|
|
cv::Mat P2;
|
|||
|
|
cv::Mat R0_rect;
|
|||
|
|
cv::Mat Tr_velo_to_cam;
|
|||
|
|
Mat trans;
|
|||
|
|
// Mat p_result;
|
|||
|
|
// Mat mimg;
|
|||
|
|
|
|||
|
|
float fx, fy, cx, cy, mDepthMapFactor;
|
|||
|
|
// cv::Mat Twc;
|
|||
|
|
std::vector<KeyFrame *> mlpSemanticKeyFrameQueue;
|
|||
|
|
// KeyFrame *mpCurrentKF;
|
|||
|
|
// PointCloud::Ptr mpPointCloud;
|
|||
|
|
|
|||
|
|
std::mutex mMutexSemanticQueue;
|
|||
|
|
|
|||
|
|
/// 当前线程是否收到了请求终止的信号
|
|||
|
|
bool mbFinishRequested;
|
|||
|
|
/// 当前线程的主函数是否已经终止
|
|||
|
|
bool mbFinished;
|
|||
|
|
// 和"线程真正结束"有关的互斥锁
|
|||
|
|
std::mutex mMutexFinish;
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|