282 lines
10 KiB
C
282 lines
10 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 FRAME_H
|
|||
|
|
#define FRAME_H
|
|||
|
|
|
|||
|
|
#include <vector>
|
|||
|
|
#include <utility>
|
|||
|
|
#include "MapPoint.h"
|
|||
|
|
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
|
|||
|
|
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
|
|||
|
|
#include "ORBVocabulary.h"
|
|||
|
|
#include "KeyFrame.h"
|
|||
|
|
#include "ORBextractor.h"
|
|||
|
|
#include "Car.h"
|
|||
|
|
|
|||
|
|
#include <opencv2/opencv.hpp>
|
|||
|
|
#include <opencv2/imgproc.hpp>
|
|||
|
|
namespace ORB_SLAM2
|
|||
|
|
{
|
|||
|
|
#define FRAME_GRID_ROWS 48
|
|||
|
|
#define FRAME_GRID_COLS 64
|
|||
|
|
|
|||
|
|
class MapPoint;
|
|||
|
|
class KeyFrame;
|
|||
|
|
|
|||
|
|
class Frame
|
|||
|
|
{
|
|||
|
|
public:
|
|||
|
|
Frame();
|
|||
|
|
|
|||
|
|
// Copy constructor.
|
|||
|
|
Frame(const Frame &frame);
|
|||
|
|
|
|||
|
|
// Constructor for stereo cameras.
|
|||
|
|
Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor *extractorLeft,
|
|||
|
|
ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
|
|||
|
|
const float &thDepth, vector<vector<double>> &vvLabel, cv::Mat &Tcw_predict, bool &state, vector<Car *> &vpCar);
|
|||
|
|
|
|||
|
|
// Constructor for RGB-D cameras.
|
|||
|
|
Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const vector<vector<double>> &vvLabel, cv::Mat &Velocity, bool &state, cv::Mat &Lastframe, float &DepthMapFactor, const cv::Mat &imRGB);
|
|||
|
|
|
|||
|
|
// Constructor for Monocular cameras.
|
|||
|
|
Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
|
|||
|
|
// Extract ORB on the image. 0 for left image and 1 for right image.
|
|||
|
|
void ExtractORB(int flag, const cv::Mat &im);
|
|||
|
|
|
|||
|
|
// Compute Bag of Words representation.
|
|||
|
|
void ComputeBoW();
|
|||
|
|
|
|||
|
|
// Set the camera pose.
|
|||
|
|
void SetPose(cv::Mat Tcw);
|
|||
|
|
|
|||
|
|
// Computes rotation, translation and camera center matrices from the camera pose.
|
|||
|
|
void UpdatePoseMatrices();
|
|||
|
|
|
|||
|
|
// Returns the camera center.
|
|||
|
|
inline cv::Mat GetCameraCenter()
|
|||
|
|
{
|
|||
|
|
return mOw.clone();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Returns inverse of rotation
|
|||
|
|
inline cv::Mat GetRotationInverse()
|
|||
|
|
{
|
|||
|
|
return mRwc.clone();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Check if a MapPoint is in the frustum of the camera
|
|||
|
|
// and fill variables of the MapPoint to be used by the tracking
|
|||
|
|
bool isInFrustum(MapPoint *pMP, float viewingCosLimit);
|
|||
|
|
|
|||
|
|
// Compute the cell of a keypoint (return false if outside the grid)
|
|||
|
|
bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
|
|||
|
|
|
|||
|
|
vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel = -1, const int maxLevel = -1) const;
|
|||
|
|
|
|||
|
|
// Search a match for each keypoint in the left image to a keypoint in the right image.
|
|||
|
|
// If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
|
|||
|
|
void ComputeStereoMatches();
|
|||
|
|
// Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
|
|||
|
|
cv::Mat UnprojectStereo(const int &i);
|
|||
|
|
|
|||
|
|
void Target_tracking(); // 目标跟踪
|
|||
|
|
|
|||
|
|
void Motion_consistency_estimation(cv::Mat &Tcl, bool &state);
|
|||
|
|
// 运动一致性检测
|
|||
|
|
// by洪
|
|||
|
|
// 绘制目标框
|
|||
|
|
void drawRotatedBox(cv::Mat &img, double &x, double &y, double &w, double &l, double &yaw, cv::Scalar &color);
|
|||
|
|
void drawDectectBox(cv::Mat Tcw, vector<cv::Mat> point, cv::Mat src, cv::Mat output);
|
|||
|
|
|
|||
|
|
// 计算IOU
|
|||
|
|
void ComputeCarMatches();
|
|||
|
|
float get_IOU(cv::Point *PointArray1, cv::Point *PointArray2);
|
|||
|
|
|
|||
|
|
float get_IOU(const vector<double> &label1, const vector<double> &label2, const cv::Mat &img, cv::Mat &T12);
|
|||
|
|
void get_bbx_with_T(const vector<double> &label1, std ::vector<cv::Point> &pts, cv::Mat &T12);
|
|||
|
|
// Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
|
|||
|
|
void get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts);
|
|||
|
|
|
|||
|
|
void T_Inverse(cv::Mat &Tcw);
|
|||
|
|
void T_Inverse(cv::Mat &T12, cv::Mat &T21);
|
|||
|
|
void bev_current_last(cv::Point &p_c, cv::Point &p_l, cv::Mat &T12); // bev视图，当前到过去的像素点坐标转换
|
|||
|
|
void drawRotatedBox(cv::Mat &img, double &x, double &y, double &w, double &l, double &yaw, cv::Scalar &color, cv::Mat &T12);
|
|||
|
|
void ComputeStereoFromRGBD(const cv::Mat &imDepth);
|
|||
|
|
void TrackObj();
|
|||
|
|
void MotionConsistencyEstimation(cv::Mat img, bool &state);
|
|||
|
|
void get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts, double scale);
|
|||
|
|
|
|||
|
|
/***************************************************************
|
|||
|
|
* @brief 目标框的世界坐标转bev的像素坐标
|
|||
|
|
* @param 世界坐标vector<cv::Mat> , 相素坐标vector<cv::Point> , 相机的位姿cv::Mat
|
|||
|
|
* @note
|
|||
|
|
* @Sample usage: 函数的使用方法
|
|||
|
|
**************************************************************/
|
|||
|
|
void world2bev(vector<cv::Mat> &vm_3d_w, vector<cv::Point> &vp_bev, cv::Mat &Tcw); // 世界坐标转bev像素
|
|||
|
|
|
|||
|
|
public:
|
|||
|
|
// Vocabulary used for relocalization.
|
|||
|
|
ORBVocabulary *mpORBvocabulary;
|
|||
|
|
|
|||
|
|
// Feature extractor. The right is used only in the stereo case.
|
|||
|
|
ORBextractor *mpORBextractorLeft, *mpORBextractorRight;
|
|||
|
|
|
|||
|
|
// Frame timestamp.
|
|||
|
|
double mTimeStamp;
|
|||
|
|
|
|||
|
|
// Calibration matrix and OpenCV distortion parameters.
|
|||
|
|
cv::Mat mK;
|
|||
|
|
static float fx;
|
|||
|
|
static float fy;
|
|||
|
|
static float cx;
|
|||
|
|
static float cy;
|
|||
|
|
static float invfx;
|
|||
|
|
static float invfy;
|
|||
|
|
|
|||
|
|
static float bev_fx;
|
|||
|
|
static float bev_fy;
|
|||
|
|
static float bev_cx;
|
|||
|
|
static float bev_cy;
|
|||
|
|
static float bev_invfx;
|
|||
|
|
static float bev_invfy;
|
|||
|
|
|
|||
|
|
cv::Mat m_bev_K;
|
|||
|
|
cv::Mat mDistCoef;
|
|||
|
|
cv::Mat mRbc; // 相机转bev的矩阵
|
|||
|
|
|
|||
|
|
// Stereo baseline multiplied by fx.
|
|||
|
|
float mbf;
|
|||
|
|
|
|||
|
|
// Stereo baseline in meters.
|
|||
|
|
float mb;
|
|||
|
|
|
|||
|
|
// Threshold close/far points. Close points are inserted from 1 view.
|
|||
|
|
// Far points are inserted as in the monocular case from 2 views.
|
|||
|
|
float mThDepth;
|
|||
|
|
|
|||
|
|
// Number of KeyPoints.
|
|||
|
|
int N;
|
|||
|
|
|
|||
|
|
vector<cv::KeyPoint> mvKeys_new;
|
|||
|
|
vector<cv::KeyPoint> mvKeysUn_new;
|
|||
|
|
vector<float> mvuRight_new;
|
|||
|
|
vector<float> mvDepth_new;
|
|||
|
|
cv::Mat mDescriptors_new;
|
|||
|
|
// Vector of keypoints (original for visualization) and undistorted (actually used by the system).
|
|||
|
|
// In the stereo case, mvKeysUn is redundant as images must be rectified.
|
|||
|
|
// In the RGB-D case, RGB images can be distorted.
|
|||
|
|
std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
|
|||
|
|
std::vector<cv::KeyPoint> mvKeysUn;
|
|||
|
|
|
|||
|
|
// Corresponding stereo coordinate and depth for each keypoint.
|
|||
|
|
// "Monocular" keypoints have a negative value.
|
|||
|
|
std::vector<float> mvuRight;
|
|||
|
|
std::vector<float> mvDepth;
|
|||
|
|
|
|||
|
|
// 建立静态匹配关系
|
|||
|
|
std::vector<int> mvCarMatches;
|
|||
|
|
|
|||
|
|
// Bag of Words Vector structures.
|
|||
|
|
DBoW2::BowVector mBowVec;
|
|||
|
|
DBoW2::FeatureVector mFeatVec;
|
|||
|
|
|
|||
|
|
// ORB descriptor, each row associated to a keypoint.
|
|||
|
|
cv::Mat mDescriptors, mDescriptorsRight;
|
|||
|
|
// MapPoints associated to keypoints, NULL pointer if no association.
|
|||
|
|
std::vector<MapPoint *> mvpMapPoints;
|
|||
|
|
|
|||
|
|
// Flag to identify outlier associations.
|
|||
|
|
std::vector<bool> mvbOutlier;
|
|||
|
|
|
|||
|
|
// Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
|
|||
|
|
static float mfGridElementWidthInv;
|
|||
|
|
static float mfGridElementHeightInv;
|
|||
|
|
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
|
|||
|
|
|
|||
|
|
// Camera pose.
|
|||
|
|
cv::Mat mTcw;
|
|||
|
|
|
|||
|
|
// Current and Next Frame id.
|
|||
|
|
static long unsigned int nNextId;
|
|||
|
|
long unsigned int mnId;
|
|||
|
|
|
|||
|
|
// Reference Keyframe.
|
|||
|
|
KeyFrame *mpReferenceKF;
|
|||
|
|
|
|||
|
|
// Scale pyramid info.
|
|||
|
|
int mnScaleLevels;
|
|||
|
|
float mfScaleFactor;
|
|||
|
|
float mfLogScaleFactor;
|
|||
|
|
vector<float> mvScaleFactors;
|
|||
|
|
vector<float> mvInvScaleFactors;
|
|||
|
|
vector<float> mvLevelSigma2;
|
|||
|
|
vector<float> mvInvLevelSigma2;
|
|||
|
|
|
|||
|
|
// Undistorted Image Bounds (computed once).
|
|||
|
|
static float mnMinX;
|
|||
|
|
static float mnMaxX;
|
|||
|
|
static float mnMinY;
|
|||
|
|
static float mnMaxY;
|
|||
|
|
|
|||
|
|
static bool mbInitialComputations;
|
|||
|
|
|
|||
|
|
// 添加动态物体label的vector,及每帧的图像 --by钟
|
|||
|
|
|
|||
|
|
vector<vector<double>> mvvLabel;
|
|||
|
|
vector<vector<double>> mvvDynamic_label;
|
|||
|
|
cv::Mat mframe;
|
|||
|
|
cv::Mat mRGB;
|
|||
|
|
cv::Mat mDepth;
|
|||
|
|
float mDepthMapFactor;
|
|||
|
|
|
|||
|
|
// 模板匹配计算相似度 --by钟
|
|||
|
|
static vector<cv::Mat> mImg1;
|
|||
|
|
static vector<cv::Mat> mImD;
|
|||
|
|
static cv::Mat mImg2;
|
|||
|
|
static vector<cv::Mat> mvImg1_Tcw;
|
|||
|
|
static cv::Mat mImg2_Tcw;
|
|||
|
|
|
|||
|
|
// Rotation, translation and camera center
|
|||
|
|
cv::Mat mRcw;
|
|||
|
|
cv::Mat mtcw;
|
|||
|
|
cv::Mat mRwc;
|
|||
|
|
cv::Mat mOw; //==mtwc
|
|||
|
|
std::vector<Car *> mvpCar;
|
|||
|
|
cv::Mat mTcw_predict;
|
|||
|
|
|
|||
|
|
private:
|
|||
|
|
// Undistort keypoints given OpenCV distortion parameters.
|
|||
|
|
// Only for the RGB-D case. Stereo must be already rectified!
|
|||
|
|
// (called in the constructor).
|
|||
|
|
void UndistortKeyPoints();
|
|||
|
|
|
|||
|
|
// Computes image bounds for the undistorted image (called in the constructor).
|
|||
|
|
void ComputeImageBounds(const cv::Mat &imLeft);
|
|||
|
|
|
|||
|
|
// Assign keypoints to the grid for speed up feature matching (called in the constructor).
|
|||
|
|
void AssignFeaturesToGrid();
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
} // namespace ORB_SLAM
|
|||
|
|
|
|||
|
|
#endif // FRAME_H
|