SLProject  4.2.000
A platform independent 3D computer graphics framework for desktop OS, Android, iOS and online in web browsers
WAIImageStabilizedOrientation Class Reference

#include <WAIImageStabilizedOrientation.h>

Public Member Functions

bool findCameraOrientationDifferenceF2F (cv::Mat imageGray, cv::Mat &imageRgb, const cv::Mat &intrinsic, float scaleToGray, bool decorate)
 
bool findCameraOrientationDifferenceF2FHorizon (const SLVec3f &horizon, cv::Mat imageGray, cv::Mat &imageRgb, const cv::Mat &intrinsic, float scaleToGray, bool decorate)
 

Private Attributes

cv::Mat _lastImageGray
 
cv::Mat _Tcw
 
float _xAngRAD = 0.f
 
float _yAngRAD = 0.f
 
float _zAngRAD = 0.f
 
std::vector< cv::KeyPoint > _lastKeyPts
 
std::vector< cv::Point2f > _lastPts
 
std::vector< cv::Point2f > _currPts
 
std::vector< cv::Point2f > _lastGoodPts
 
std::vector< cv::Point2f > _currGoodPts
 
std::vector< uchar > _inliers
 
std::vector< float > _err
 
int _fIniThFAST = 30
 
int _fMinThFAST = 7
 

Detailed Description

Definition at line 7 of file WAIImageStabilizedOrientation.h.

Member Function Documentation

◆ findCameraOrientationDifferenceF2F()

bool WAIImageStabilizedOrientation::findCameraOrientationDifferenceF2F ( cv::Mat  imageGray,
cv::Mat &  imageRgb,
const cv::Mat &  intrinsic,
float  scaleToGray,
bool  decorate 
)

Definition at line 111 of file WAIImageStabilizedOrientation.cpp.

116 {
117  //initialization
118  if (_lastImageGray.empty())
119  {
120  _lastImageGray = imageGray.clone(); //todo: maybe clone not needed
121  return false;
122  }
123 
124  //extract fast corners on current image
125  _lastPts.clear();
126  _lastKeyPts.clear();
127  cv::FAST(_lastImageGray, _lastKeyPts, _fIniThFAST, true);
128  _lastPts.reserve(_lastKeyPts.size());
129  for (int i = 0; i < _lastKeyPts.size(); i++)
130  _lastPts.push_back(_lastKeyPts[i].pt);
131  //Utils::log("WAI", "num features extracted: %d", _lastPts.size());
132 
133  if (_lastPts.size() < 10)
134  {
135  _lastImageGray = imageGray.clone();
136  return false;
137  }
138 
140  imageGray,
141  _lastPts,
142  _currPts,
143  _inliers,
144  _err);
145 
147  _currPts,
148  _lastGoodPts,
149  _currGoodPts,
150  _inliers,
151  _err);
152 
153  float xAngRAD, yAngRAD, zAngRAD;
154  bool success = F2FTransform::estimateRotXYZ(intrinsic, _lastGoodPts, _currGoodPts, xAngRAD, yAngRAD, zAngRAD, _inliers);
155 
156  if (success)
157  {
158  if (decorate)
159  {
160  std::vector<cv::Point2f> _lastRealGoodPts;
161  std::vector<cv::Point2f> _currRealGoodPts;
163  _currGoodPts,
164  _lastRealGoodPts,
165  _currRealGoodPts,
166  _inliers,
167  _err);
168 
169  cv::Point2f r(2.f, 2.f);
170  for (unsigned int i = 0; i < _lastRealGoodPts.size(); i++)
171  {
172  cv::Point2f p1 = _lastRealGoodPts[i] * scaleToGray;
173  cv::Point2f p2 = _currRealGoodPts[i] * scaleToGray;
174  cv::line(imageRgb, p1, p2, cv::Scalar(0, 255, 0));
175  cv::rectangle(imageRgb, (p1 - r) * scaleToGray, (p1 + r) * scaleToGray, CV_RGB(255, 0, 0));
176  }
177  }
178 
179  cv::Mat Rx, Ry, Rz, Tcw;
180  _xAngRAD += xAngRAD;
181  _yAngRAD += yAngRAD;
182  _zAngRAD += zAngRAD;
183  //std::cout << "_xAngRAD: " << _xAngRAD * RAD2DEG << std::endl;
184  //std::cout << "_yAngRAD: " << _yAngRAD * RAD2DEG << std::endl;
185  //std::cout << "_zAngRAD: " << _zAngRAD * RAD2DEG << std::endl;
186  Utils::log("WAI track", "x: %.0f y: %.0f z: %.0f", _xAngRAD * Utils::RAD2DEG, _yAngRAD * Utils::RAD2DEG, _zAngRAD * Utils::RAD2DEG);
187  /*
188  F2FTransform::eulerToMat(_xAngRAD, _yAngRAD, _zAngRAD, Rx, Ry, Rz);
189  Tcw = Rx * Ry * Rz;
190  cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
191  //?????????????????
192  //pose.at<float>(2, 3) = -1.5; //?????????????????
193  pose = Tcw * pose;
194  //pos.copyTo(_objectViewMat);
195  */
196  }
197 
198  _lastImageGray = imageGray.clone();
199 
200  return success;
201 }
static bool estimateRotXYZ(const cv::Mat &K, const std::vector< cv::Point2f > &p1, const std::vector< cv::Point2f > &p2, float &xAngRAD, float &yAngRAD, float &zAngRAD, std::vector< uchar > &inliers)
static float filterPoints(const std::vector< cv::Point2f > &p1, const std::vector< cv::Point2f > &p2, std::vector< cv::Point2f > &goodP1, std::vector< cv::Point2f > &goodP2, std::vector< uchar > &inliers, std::vector< float > &err)
static void opticalFlowMatch(const cv::Mat &f1Gray, const cv::Mat &f2Gray, std::vector< cv::Point2f > &p1, std::vector< cv::Point2f > &p2, std::vector< uchar > &inliers, std::vector< float > &err)
std::vector< cv::KeyPoint > _lastKeyPts
static const float RAD2DEG
Definition: Utils.h:238
void log(const char *tag, const char *format,...)
logs a formatted string platform independently
Definition: Utils.cpp:1103

◆ findCameraOrientationDifferenceF2FHorizon()

bool WAIImageStabilizedOrientation::findCameraOrientationDifferenceF2FHorizon ( const SLVec3f horizon,
cv::Mat  imageGray,
cv::Mat &  imageRgb,
const cv::Mat &  intrinsic,
float  scaleToGray,
bool  decorate 
)

Definition at line 10 of file WAIImageStabilizedOrientation.cpp.

16 {
17  //initialization
18  if (_lastImageGray.empty())
19  {
20  _lastImageGray = imageGray.clone(); //todo: maybe clone not needed
21  return false;
22  }
23 
24  //extract fast corners on current image
25  _lastPts.clear();
26  _lastKeyPts.clear();
27  cv::FAST(_lastImageGray, _lastKeyPts, _fIniThFAST, true);
28  _lastPts.reserve(_lastKeyPts.size());
29  for (int i = 0; i < _lastKeyPts.size(); i++)
30  _lastPts.push_back(_lastKeyPts[i].pt);
31  //Utils::log("WAI", "num features extracted: %d", _lastPts.size());
32 
33  if (_lastPts.size() < 10)
34  {
35  _lastImageGray = imageGray.clone();
36  return false;
37  }
38 
40  imageGray,
41  _lastPts,
42  _currPts,
43  _inliers,
44  _err);
45 
47  _currPts,
50  _inliers,
51  _err);
52 
53  float xAngRAD = 0.0f, yAngRAD = 0.0f, zAngRAD = 0.0f;
54 
55  //estimate z from horizon
56  bool success = F2FTransform::estimateRotXY(intrinsic,
59  xAngRAD,
60  yAngRAD,
61  zAngRAD,
62  _inliers);
63 
64  if (success)
65  {
66  if (decorate)
67  {
68  std::vector<cv::Point2f> _lastRealGoodPts;
69  std::vector<cv::Point2f> _currRealGoodPts;
72  _lastRealGoodPts,
73  _currRealGoodPts,
74  _inliers,
75  _err);
76 
77  cv::Point2f r(2.f, 2.f);
78  for (unsigned int i = 0; i < _lastRealGoodPts.size(); i++)
79  {
80  cv::Point2f p1 = _lastRealGoodPts[i] * scaleToGray;
81  cv::Point2f p2 = _currRealGoodPts[i] * scaleToGray;
82  cv::line(imageRgb, p1, p2, cv::Scalar(0, 255, 0));
83  cv::rectangle(imageRgb, (p1 - r) * scaleToGray, (p1 + r) * scaleToGray, CV_RGB(255, 0, 0));
84  }
85  }
86 
87  cv::Mat Rx, Ry, Rz, Tcw;
88  _xAngRAD += xAngRAD;
89  _yAngRAD += yAngRAD;
90  _zAngRAD += zAngRAD;
91  //std::cout << "_xAngRAD: " << _xAngRAD * RAD2DEG << std::endl;
92  //std::cout << "_yAngRAD: " << _yAngRAD * RAD2DEG << std::endl;
93  //std::cout << "_zAngRAD: " << _zAngRAD * RAD2DEG << std::endl;
94  Utils::log("WAI track", "x: %.0f y: %.0f z: %.0f", _xAngRAD * Utils::RAD2DEG, _yAngRAD * Utils::RAD2DEG, _zAngRAD * Utils::RAD2DEG);
95  /*
96  F2FTransform::eulerToMat(_xAngRAD, _yAngRAD, _zAngRAD, Rx, Ry, Rz);
97  Tcw = Rx * Ry * Rz;
98  cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
99  //?????????????????
100  //pose.at<float>(2, 3) = -1.5; //?????????????????
101  pose = Tcw * pose;
102  //pos.copyTo(_objectViewMat);
103  */
104  }
105 
106  _lastImageGray = imageGray.clone();
107 
108  return success;
109 }
static bool estimateRotXY(const cv::Mat &K, const std::vector< cv::Point2f > &p1, const std::vector< cv::Point2f > &p2, float &xAngRAD, float &yAngRAD, const float zAngRAD, std::vector< uchar > &inliers)

Member Data Documentation

◆ _currGoodPts

std::vector<cv::Point2f> WAIImageStabilizedOrientation::_currGoodPts
private

Definition at line 33 of file WAIImageStabilizedOrientation.h.

◆ _currPts

std::vector<cv::Point2f> WAIImageStabilizedOrientation::_currPts
private

Definition at line 31 of file WAIImageStabilizedOrientation.h.

◆ _err

std::vector<float> WAIImageStabilizedOrientation::_err
private

Definition at line 35 of file WAIImageStabilizedOrientation.h.

◆ _fIniThFAST

int WAIImageStabilizedOrientation::_fIniThFAST = 30
private

Definition at line 37 of file WAIImageStabilizedOrientation.h.

◆ _fMinThFAST

int WAIImageStabilizedOrientation::_fMinThFAST = 7
private

Definition at line 38 of file WAIImageStabilizedOrientation.h.

◆ _inliers

std::vector<uchar> WAIImageStabilizedOrientation::_inliers
private

Definition at line 34 of file WAIImageStabilizedOrientation.h.

◆ _lastGoodPts

std::vector<cv::Point2f> WAIImageStabilizedOrientation::_lastGoodPts
private

Definition at line 32 of file WAIImageStabilizedOrientation.h.

◆ _lastImageGray

cv::Mat WAIImageStabilizedOrientation::_lastImageGray
private

Definition at line 23 of file WAIImageStabilizedOrientation.h.

◆ _lastKeyPts

std::vector<cv::KeyPoint> WAIImageStabilizedOrientation::_lastKeyPts
private

Definition at line 29 of file WAIImageStabilizedOrientation.h.

◆ _lastPts

std::vector<cv::Point2f> WAIImageStabilizedOrientation::_lastPts
private

Definition at line 30 of file WAIImageStabilizedOrientation.h.

◆ _Tcw

cv::Mat WAIImageStabilizedOrientation::_Tcw
private

Definition at line 24 of file WAIImageStabilizedOrientation.h.

◆ _xAngRAD

float WAIImageStabilizedOrientation::_xAngRAD = 0.f
private

Definition at line 25 of file WAIImageStabilizedOrientation.h.

◆ _yAngRAD

float WAIImageStabilizedOrientation::_yAngRAD = 0.f
private

Definition at line 26 of file WAIImageStabilizedOrientation.h.

◆ _zAngRAD

float WAIImageStabilizedOrientation::_zAngRAD = 0.f
private

Definition at line 27 of file WAIImageStabilizedOrientation.h.


The documentation for this class was generated from the following files: