-
Notifications
You must be signed in to change notification settings - Fork 7
/
PoseEstimation.h
46 lines (42 loc) · 1.22 KB
/
PoseEstimation.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#ifndef POSEESTIMATION_H
#define POSEESTIMATION_H
#include <opencv2/imgproc.hpp>
class PoseEstimation
{
public:
PoseEstimation();
~PoseEstimation();
void addImagePoints(std::vector<cv::Point2d> &imagePoints);
void solve(cv::Mat &image, std::vector<cv::Point2d> &imagePoints);
cv::Mat getRotationMatrix() const;
cv::Mat getRotationVector() const;
cv::Mat getTranslationVector() const;
protected:
void drawBasis(cv::Mat &image, std::vector<cv::Point2d> &imagePoints);
private:
struct CameraParams {
CameraParams(cv::Mat &image) {
this->dmax = MAX(image.rows, image.cols);
this->cmat = (cv::Mat_<double>(3, 3) << dmax, 0, image.cols / 2.0,
0, dmax, image.rows / 2.0,
0, 0, 1.0);
memset(distCoeffs, 0, sizeof distCoeffs);
}
int dmax;
cv::Mat cmat;
double distCoeffs[4];
};
cv::Mat _objPoints;
cv::Mat _imgPoints;
double _d[9] = {
1.f,0.f,0.f,
0.f,-1.f,0.f,
0.f,0.f,-1.f };
std::vector<double> _rv;
std::vector<double> _tv;
cv::Mat _rvec;
cv::Mat _tvec;
cv::Mat _rmat;
void initObjPoints();
};
#endif // POSEESTIMATION_H