#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.

 {
     //initialization
     if (_lastImageGray.empty())
     {
         _lastImageGray = imageGray.clone(); //todo: maybe clone not needed
         return false;
     }
  
     //extract fast corners on current image
     _lastPts.clear();
     _lastKeyPts.clear();
     cv::FAST(_lastImageGray, _lastKeyPts, _fIniThFAST, true);
     _lastPts.reserve(_lastKeyPts.size());
     for (int i = 0; i < _lastKeyPts.size(); i++)
         _lastPts.push_back(_lastKeyPts[i].pt);
     //Utils::log("WAI", "num features extracted: %d", _lastPts.size());
  
     if (_lastPts.size() < 10)
     {
         _lastImageGray = imageGray.clone();
         return false;
     }
  
     F2FTransform::opticalFlowMatch(_lastImageGray,
                                    imageGray,
                                    _lastPts,
                                    _currPts,
                                    _inliers,
                                    _err);
  
     F2FTransform::filterPoints(_lastPts,
                                _currPts,
                                _lastGoodPts,
                                _currGoodPts,
                                _inliers,
                                _err);
  
     float xAngRAD, yAngRAD, zAngRAD;
     bool  success = F2FTransform::estimateRotXYZ(intrinsic, _lastGoodPts, _currGoodPts, xAngRAD, yAngRAD, zAngRAD, _inliers);
  
     if (success)
     {
         if (decorate)
         {
             std::vector<cv::Point2f> _lastRealGoodPts;
             std::vector<cv::Point2f> _currRealGoodPts;
             F2FTransform::filterPoints(_lastGoodPts,
                                        _currGoodPts,
                                        _lastRealGoodPts,
                                        _currRealGoodPts,
                                        _inliers,
                                        _err);
  
             cv::Point2f r(2.f, 2.f);
             for (unsigned int i = 0; i < _lastRealGoodPts.size(); i++)
             {
                 cv::Point2f p1 = _lastRealGoodPts[i] * scaleToGray;
                 cv::Point2f p2 = _currRealGoodPts[i] * scaleToGray;
                 cv::line(imageRgb, p1, p2, cv::Scalar(0, 255, 0));
                 cv::rectangle(imageRgb, (p1 - r) * scaleToGray, (p1 + r) * scaleToGray, CV_RGB(255, 0, 0));
             }
         }
  
         cv::Mat Rx, Ry, Rz, Tcw;
         _xAngRAD += xAngRAD;
         _yAngRAD += yAngRAD;
         _zAngRAD += zAngRAD;
         //std::cout << "_xAngRAD: " << _xAngRAD * RAD2DEG << std::endl;
         //std::cout << "_yAngRAD: " << _yAngRAD * RAD2DEG << std::endl;
         //std::cout << "_zAngRAD: " << _zAngRAD * RAD2DEG << std::endl;
         Utils::log("WAI track", "x: %.0f y: %.0f z: %.0f", _xAngRAD * Utils::RAD2DEG, _yAngRAD * Utils::RAD2DEG, _zAngRAD * Utils::RAD2DEG);
         /*
         F2FTransform::eulerToMat(_xAngRAD, _yAngRAD, _zAngRAD, Rx, Ry, Rz);
         Tcw = Rx * Ry * Rz;
         cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
         //?????????????????
         //pose.at<float>(2, 3) = -1.5; //?????????????????
         pose = Tcw * pose;
         //pos.copyTo(_objectViewMat);
          */
     }
  
     _lastImageGray = imageGray.clone();
  
     return success;
 }

◆ 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.

 {
     //initialization
     if (_lastImageGray.empty())
     {
         _lastImageGray = imageGray.clone(); //todo: maybe clone not needed
         return false;
     }
  
     //extract fast corners on current image
     _lastPts.clear();
     _lastKeyPts.clear();
     cv::FAST(_lastImageGray, _lastKeyPts, _fIniThFAST, true);
     _lastPts.reserve(_lastKeyPts.size());
     for (int i = 0; i < _lastKeyPts.size(); i++)
         _lastPts.push_back(_lastKeyPts[i].pt);
     //Utils::log("WAI", "num features extracted: %d", _lastPts.size());
  
     if (_lastPts.size() < 10)
     {
         _lastImageGray = imageGray.clone();
         return false;
     }
  
     F2FTransform::opticalFlowMatch(_lastImageGray,
                                    imageGray,
                                    _lastPts,
                                    _currPts,
                                    _inliers,
                                    _err);
  
     F2FTransform::filterPoints(_lastPts,
                                _currPts,
                                _lastGoodPts,
                                _currGoodPts,
                                _inliers,
                                _err);
  
     float xAngRAD = 0.0f, yAngRAD = 0.0f, zAngRAD = 0.0f;
  
     //estimate z from horizon
     bool success = F2FTransform::estimateRotXY(intrinsic,
                                                _lastGoodPts,
                                                _currGoodPts,
                                                xAngRAD,
                                                yAngRAD,
                                                zAngRAD,
                                                _inliers);
  
     if (success)
     {
         if (decorate)
         {
             std::vector<cv::Point2f> _lastRealGoodPts;
             std::vector<cv::Point2f> _currRealGoodPts;
             F2FTransform::filterPoints(_lastGoodPts,
                                        _currGoodPts,
                                        _lastRealGoodPts,
                                        _currRealGoodPts,
                                        _inliers,
                                        _err);
  
             cv::Point2f r(2.f, 2.f);
             for (unsigned int i = 0; i < _lastRealGoodPts.size(); i++)
             {
                 cv::Point2f p1 = _lastRealGoodPts[i] * scaleToGray;
                 cv::Point2f p2 = _currRealGoodPts[i] * scaleToGray;
                 cv::line(imageRgb, p1, p2, cv::Scalar(0, 255, 0));
                 cv::rectangle(imageRgb, (p1 - r) * scaleToGray, (p1 + r) * scaleToGray, CV_RGB(255, 0, 0));
             }
         }
  
         cv::Mat Rx, Ry, Rz, Tcw;
         _xAngRAD += xAngRAD;
         _yAngRAD += yAngRAD;
         _zAngRAD += zAngRAD;
         //std::cout << "_xAngRAD: " << _xAngRAD * RAD2DEG << std::endl;
         //std::cout << "_yAngRAD: " << _yAngRAD * RAD2DEG << std::endl;
         //std::cout << "_zAngRAD: " << _zAngRAD * RAD2DEG << std::endl;
         Utils::log("WAI track", "x: %.0f y: %.0f z: %.0f", _xAngRAD * Utils::RAD2DEG, _yAngRAD * Utils::RAD2DEG, _zAngRAD * Utils::RAD2DEG);
         /*
         F2FTransform::eulerToMat(_xAngRAD, _yAngRAD, _zAngRAD, Rx, Ry, Rz);
         Tcw = Rx * Ry * Rz;
         cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
         //?????????????????
         //pose.at<float>(2, 3) = -1.5; //?????????????????
         pose = Tcw * pose;
         //pos.copyTo(_objectViewMat);
          */
     }
  
     _lastImageGray = imageGray.clone();
  
     return success;
 }