SLProject  4.2.000
A platform independent 3D computer graphics framework for desktop OS, Android, iOS and online in web browsers
Static Public Member Functions | Static Private Member Functions | List of all members
F2FTransform Class Reference

#include <F2FTransform.h>

Static Public Member Functions

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)
 
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 bool estimateRot (const cv::Mat K, std::vector< cv::Point2f > &p1, std::vector< cv::Point2f > &p2, float &yaw, float &pitch, float &roll)
 
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 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)
 
static void eulerToMat (float xAngRAD, float yAngRAD, float zAngRAD, cv::Mat &Rx, cv::Mat &Ry, cv::Mat &Rz)
 

Static Private Member Functions

static cv::Mat eigen2cv (Eigen::Matrix3f m)
 

Detailed Description

Definition at line 11 of file F2FTransform.h.

Member Function Documentation

◆ eigen2cv()

cv::Mat F2FTransform::eigen2cv ( Eigen::Matrix3f  m)
staticprivate

Definition at line 266 of file F2FTransform.cpp.

267 {
268  cv::Mat r;
269  r = (cv::Mat_<float>(3, 3) << m(0), m(3), m(6), m(1), m(4), m(7), m(2), m(5), m(8));
270  return r;
271 }

◆ estimateRot()

bool F2FTransform::estimateRot ( const cv::Mat  K,
std::vector< cv::Point2f > &  p1,
std::vector< cv::Point2f > &  p2,
float &  yaw,
float &  pitch,
float &  roll 
)
static

Definition at line 93 of file F2FTransform.cpp.

99 {
100  if (p1.size() < 10)
101  return false;
102 
103  cv::Mat H = estimateAffinePartial2D(p1, p2);
104  float zrot = (float)atan2(H.at<double>(1, 0), H.at<double>(0, 0));
105  float dx = 0; //H.at<double>(0, 2);
106  float dy = 0; //H.at<double>(1, 2);
107  //Compute dx dy (estimageAffinePartial doesn't give right result when rotating on z axis)
108  for (int i = 0; i < p1.size(); i++)
109  {
110  dx += p2[i].x - p1[i].x;
111  dy += p2[i].y - p1[i].y;
112  }
113  dx /= (float)p1.size();
114  dy /= (float)p1.size();
115 
116  Eigen::Vector3f v1(0, 0, 1.0);
117  Eigen::Vector3f vx(dx, 0, (float)K.at<double>(0, 0));
118  Eigen::Vector3f vy(0, dy, (float)K.at<double>(0, 0));
119  vx.normalize();
120  vy.normalize();
121 
122  float xrot = -acos(v1.dot(vx));
123  float yrot = -acos(v1.dot(vy));
124 
125  roll = -zrot;
126 
127  if (vx.dot(Eigen::Vector3f::UnitX()) > 0)
128  pitch = xrot;
129  else
130  pitch = -xrot;
131 
132  if (vy.dot(Eigen::Vector3f::UnitY()) > 0)
133  yaw = yrot;
134  else
135  yaw = -yrot;
136 
137  return true;
138 }

◆ estimateRotXY()

bool F2FTransform::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 
)
static

Definition at line 187 of file F2FTransform.cpp.

194 {
195  if (p1.size() < 10)
196  return false;
197 
198  //relate points to optival center
199  //HINT: void at(int row, int column)
200  double cx = K.at<double>(0, 2);
201  double cy = K.at<double>(1, 2);
202  //rotation matrix
203 
204  cv::Point2f c((float)cx, (float)cy); //optical center
205  std::vector<cv::Point2f> p1C = p1;
206  std::vector<cv::Point2f> p2C = p2;
207  for (int i = 0; i < p1.size(); i++)
208  {
209  p1C[i] -= c;
210  p2C[i] -= c;
211  //rotate points about center
212  }
213 
214  //estimate median shift
215 
216  inliers.clear();
217  //estimate homography (gives us frame b w.r.t frame a)
218  cv::Mat aHb = cv::estimateAffinePartial2D(p1C, p2C, inliers);
219  if (aHb.empty())
220  return false;
221  //std::cout << "aHb: " << aHb << std::endl;
222 
223  //express translational part in aHb relative to a coordinate frame b' that was rotated with rotational part
224  //cv::Mat bRa = aHb.rowRange(0, 2).colRange(0, 2).t(); //extract and express rotation from rotated frame
225  //cv::Mat aTR = aHb.col(2);
226  //cv::Mat bTR = bRa * aTR;
227 
228  //rotation about z-axis: It points into the image plane, so we have to invert the sign
229  //zAngRAD = atan2(aHb.at<double>(1, 0), aHb.at<double>(0, 0));
230  //rotation around y-axis about x-offset: it points down in cv image, so we have to invert the sign
231  yAngRAD = (float)atan(aHb.at<double>(0, 2) / K.at<double>(0, 0));
232  //rotation around x-axis about y-offset: it points down in cv image, so we have to invert the sign
233  xAngRAD = (float)atan(aHb.at<double>(1, 2) / K.at<double>(1, 1));
234 
235  //std::cout << "xoff: "<< bTR.at<double>(0) << std::endl;
236  //std::cout << "yoff: "<< bTR.at<double>(1) << std::endl;
237 
238  //std::cout << "zAngDEG: "<< zAngDEG << std::endl;
239  //std::cout << "yAngDEG: "<< yAngDEG << std::endl;
240  //std::cout << "xAngDEG: "<< xAngDEG << std::endl;
241 
242  return true;
243 }

◆ estimateRotXYZ()

bool F2FTransform::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

Definition at line 140 of file F2FTransform.cpp.

147 {
148  if (p1.size() < 10)
149  return false;
150 
151  //relate points to optical center
152  //HINT: void at(int row, int column)
153  double cx = K.at<double>(0, 2);
154  double cy = K.at<double>(1, 2);
155 
156  cv::Point2f c((float)cx, (float)cy); //optical center
157  std::vector<cv::Point2f> p1C = p1;
158  std::vector<cv::Point2f> p2C = p2;
159  for (int i = 0; i < p1.size(); i++)
160  {
161  p1C[i] -= c;
162  p2C[i] -= c;
163  }
164 
165  inliers.clear();
166  //estimate homography (gives us frame b w.r.t frame a)
167  cv::Mat aHb = cv::estimateAffinePartial2D(p1C, p2C, inliers);
168  if (aHb.empty())
169  return false;
170  //std::cout << "aHb: " << aHb << std::endl;
171 
172  //express translational part in aHb relative to a coordinate frame b' that was rotated with rotational part
173  cv::Mat bRa = aHb.rowRange(0, 2).colRange(0, 2).t(); //extract and express rotation from rotated frame
174  cv::Mat aTR = aHb.col(2);
175  cv::Mat bTR = bRa * aTR;
176 
177  //rotation about z-axis: It points into the image plane, so we have to invert the sign
178  zAngRAD = (float)atan2(aHb.at<double>(1, 0), aHb.at<double>(0, 0));
179  //rotation around y-axis about x-offset: it points down in cv image, so we have to invert the sign
180  yAngRAD = (float)atan(bTR.at<double>(0) / K.at<double>(0, 0));
181  //rotation around x-axis about y-offset: it points down in cv image, so we have to invert the sign
182  xAngRAD = (float)atan(bTR.at<double>(1) / K.at<double>(1, 1));
183 
184  return true;
185 }

◆ eulerToMat()

void F2FTransform::eulerToMat ( float  xAngRAD,
float  yAngRAD,
float  zAngRAD,
cv::Mat &  Rx,
cv::Mat &  Ry,
cv::Mat &  Rz 
)
static

Definition at line 245 of file F2FTransform.cpp.

246 {
247  Eigen::Matrix3f mx, my, mz;
248  mx = Eigen::AngleAxisf(xAngRAD, Eigen::Vector3f::UnitX());
249  my = Eigen::AngleAxisf(yAngRAD, Eigen::Vector3f::UnitY());
250  mz = Eigen::AngleAxisf(zAngRAD, Eigen::Vector3f::UnitZ());
251 
252  cv::Mat eRx = eigen2cv(mx);
253  cv::Mat eRy = eigen2cv(my);
254  cv::Mat eRz = eigen2cv(mz);
255 
256  Rx = cv::Mat::eye(4, 4, CV_32F);
257  eRx.copyTo(Rx.rowRange(0, 3).colRange(0, 3));
258 
259  Ry = cv::Mat::eye(4, 4, CV_32F);
260  eRy.copyTo(Ry.rowRange(0, 3).colRange(0, 3));
261 
262  Rz = cv::Mat::eye(4, 4, CV_32F);
263  eRz.copyTo(Rz.rowRange(0, 3).colRange(0, 3));
264 }
F2FTransform::eigen2cv
static cv::Mat eigen2cv(Eigen::Matrix3f m)
Definition: F2FTransform.cpp:266

◆ filterPoints()

float F2FTransform::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

Definition at line 66 of file F2FTransform.cpp.

72 {
73  if (p1.size() == 0)
74  return 0;
75  goodP1.clear();
76  goodP2.clear();
77  goodP1.reserve(p1.size());
78  goodP2.reserve(p1.size());
79  float avgMotion = 0;
80 
81  for (int i = 0; i < p1.size(); i++)
82  {
83  if (inliers[i] && err[i] < 10.0)
84  {
85  goodP1.push_back(p1[i]);
86  goodP2.push_back(p2[i]);
87  avgMotion += (float)cv::norm(p1[i] - p2[i]);
88  }
89  }
90  return avgMotion / goodP1.size();
91 }

◆ opticalFlowMatch()

void F2FTransform::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 
)
static

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

Definition at line 30 of file F2FTransform.cpp.

36 {
37  if (p1.size() < 10)
38  return;
39 
40  cv::TermCriteria criteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 10, 0.03);
41 
42  p2.clear();
43  inliers.clear();
44  err.clear();
45 
46  p2.reserve(p1.size());
47  inliers.reserve(p1.size());
48  err.reserve(p1.size());
49 
50  cv::Size winSize(11, 11);
51 
52  cv::calcOpticalFlowPyrLK(
53  f1Gray,
54  f2Gray,
55  p1, // Previous and current keypoints coordinates.The latter will be
56  p2, // expanded if more good coordinates are detected during OptFlow
57  inliers, // Output vector for keypoint correspondences (1 = match found)
58  err, // Error size for each flow
59  winSize, // Search window for each pyramid level
60  1, // Max levels of pyramid creation
61  criteria, // Configuration from above
62  0, // Additional flags
63  0.001); // Minimal Eigen threshold
64 }
The documentation for this class was generated from the following files: