boarddetector.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 #include "boarddetector.h"
29 #define _USE_MATH_DEFINES
30 #include <math.h>
31 #include <cstdlib>
32 #include <ctime>
33 #include <cassert>
34 #include <fstream>
35 #include <opencv2/calib3d/calib3d.hpp>
36 using namespace std;
37 using namespace cv;
38 namespace aruco {
41 BoardDetector::BoardDetector(bool setYPerpendicular) {
42  _setYPerpendicular = setYPerpendicular;
43  _areParamsSet = false;
44  repj_err_thres = -1;
45 }
49 void BoardDetector::setParams(const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters) {
50  _camParams = cp;
51  _markerSize = markerSizeMeters;
52  _bconf = bc;
53  _areParamsSet = true;
54 }
59 void BoardDetector::setParams(const BoardConfiguration &bc) {
60  _bconf = bc;
61  _areParamsSet = true;
62 }
63 
68 float BoardDetector::detect(const cv::Mat &im) throw(cv::Exception) {
69  _mdetector.detect(im, _vmarkers);
70 
71  float res;
72 
73  if (_camParams.isValid())
74  res = detect(_vmarkers, _bconf, _boardDetected, _camParams.CameraMatrix, _camParams.Distorsion, _markerSize);
75  else
76  res = detect(_vmarkers, _bconf, _boardDetected);
77  return res;
78 }
83 float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, const CameraParameters &cp,
84  float markerSizeMeters) throw(cv::Exception) {
85  return detect(detectedMarkers, BConf, Bdetected, cp.CameraMatrix, cp.Distorsion, markerSizeMeters);
86 }
91 float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix, Mat distCoeff,
92  float markerSizeMeters) throw(cv::Exception) {
93  if (BConf.size() == 0)
94  throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty", __FILE__, __LINE__);
95  if (BConf[0].size() < 2)
96  throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty 2", __FILE__, __LINE__);
97  // compute the size of the markers in meters, which is used for some routines(mostly drawing)
98  float ssize;
99  if (BConf.mInfoType == BoardConfiguration::PIX && markerSizeMeters > 0)
100  ssize = markerSizeMeters;
101  else if (BConf.mInfoType == BoardConfiguration::METERS) {
102  ssize = cv::norm(BConf[0][0] - BConf[0][1]);
103  }
104 
105  // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
106  Bdetected.clear();
108  for (unsigned int i = 0; i < detectedMarkers.size(); i++) {
109  int idx = BConf.getIndexOfMarkerId(detectedMarkers[i].id);
110  if (idx != -1) {
111  Bdetected.push_back(detectedMarkers[i]);
112  Bdetected.back().ssize = ssize;
113  }
114  }
115  // copy configuration
116  Bdetected.conf = BConf;
117  //
118 
119  bool hasEnoughInfoForRTvecCalculation = false;
120  if (Bdetected.size() >= 1) {
121  if (camMatrix.rows != 0) {
122  if (markerSizeMeters > 0 && BConf.mInfoType == BoardConfiguration::PIX)
123  hasEnoughInfoForRTvecCalculation = true;
124  else if (BConf.mInfoType == BoardConfiguration::METERS)
125  hasEnoughInfoForRTvecCalculation = true;
126  }
127  }
128 
129  // calculate extrinsic if there is information for that
130  if (hasEnoughInfoForRTvecCalculation) {
131 
132  // calculate the size of the markers in meters if expressed in pixels
133  double marker_meter_per_pix = 0;
134  if (BConf.mInfoType == BoardConfiguration::PIX)
135  marker_meter_per_pix = markerSizeMeters / cv::norm(BConf[0][0] - BConf[0][1]);
136  else
137  marker_meter_per_pix = 1; // to avoind interferring the process below
138 
139  // now, create the matrices for finding the extrinsics
140  vector< cv::Point3f > objPoints;
141  vector< cv::Point2f > imagePoints;
142  for (size_t i = 0; i < Bdetected.size(); i++) {
143  int idx = Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
144  assert(idx != -1);
145  for (int p = 0; p < 4; p++) {
146  imagePoints.push_back(Bdetected[i][p]);
147  const aruco::MarkerInfo &Minfo = Bdetected.conf.getMarkerInfo(Bdetected[i].id);
148  objPoints.push_back(Minfo[p] * marker_meter_per_pix);
149  // cout<<objPoints.back()<<endl;
150  }
151  }
152  if (distCoeff.total() == 0)
153  distCoeff = cv::Mat::zeros(1, 4, CV_32FC1);
154 
155  // for(size_t i=0;i< imagePoints.size();i++){
156  // cout<<objPoints[i]<<" "<<imagePoints[i]<<endl;
157  // }
158  // cout<<"cam="<<camMatrix<<" "<<distCoeff<<endl;
159  cv::Mat rvec, tvec;
160  cv::solvePnP(objPoints, imagePoints, camMatrix, distCoeff, rvec, tvec);
161  rvec.convertTo(Bdetected.Rvec, CV_32FC1);
162  tvec.convertTo(Bdetected.Tvec, CV_32FC1);
163  // cout<<rvec<< " "<<tvec<<" _setYPerpendicular="<<_setYPerpendicular<<endl;
164 
165  {
166  vector< cv::Point2f > reprojected;
167  cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);
168  double errSum = 0;
169  // check now the reprojection error and
170  for (size_t i = 0; i < reprojected.size(); i++) {
171  errSum += cv::norm(reprojected[i] - imagePoints[i]);
172  }
173  // cout<<"AAA RE="<<errSum/double ( reprojected.size() ) <<endl;
174  }
175  // now, do a refinement and remove points whose reprojection error is above a threshold, then repeat calculation with the rest
176  if (repj_err_thres > 0) {
177  vector< cv::Point2f > reprojected;
178  cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);
179 
180  vector< int > pointsThatPassTest; // indices
181  // check now the reprojection error and
182  for (size_t i = 0; i < reprojected.size(); i++) {
183  float err = cv::norm(reprojected[i] - imagePoints[i]);
184  if (err < repj_err_thres)
185  pointsThatPassTest.push_back(i);
186  }
187  // cerr<<"Number of points after reprjection test "<<pointsThatPassTest.size() <<"/"<<objPoints.size() <<endl;
188  // copy these data to another vectors and repeat
189  vector< cv::Point3f > objPoints_filtered;
190  vector< cv::Point2f > imagePoints_filtered;
191  for (size_t i = 0; i < pointsThatPassTest.size(); i++) {
192  objPoints_filtered.push_back(objPoints[pointsThatPassTest[i]]);
193  imagePoints_filtered.push_back(imagePoints[pointsThatPassTest[i]]);
194  }
195 
196  cv::solvePnP(objPoints_filtered, imagePoints_filtered, camMatrix, distCoeff, rvec, tvec);
197  rvec.convertTo(Bdetected.Rvec, CV_32FC1);
198  tvec.convertTo(Bdetected.Tvec, CV_32FC1);
199  }
200 
201 
202  // now, rotate 90 deg in X so that Y axis points up
203  if (_setYPerpendicular)
204  rotateXAxis(Bdetected.Rvec);
205  // cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
206  // cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
207  }
208 
209  float prob = float(Bdetected.size()) / double(Bdetected.conf.size());
210  return prob;
211 }
212 
213 void BoardDetector::rotateXAxis(Mat &rotation) {
214  cv::Mat R(3, 3, CV_32FC1);
215  Rodrigues(rotation, R);
216  // create a rotation matrix for x axis
217  cv::Mat RX = cv::Mat::eye(3, 3, CV_32FC1);
218  float angleRad = -M_PI / 2;
219  RX.at< float >(1, 1) = cos(angleRad);
220  RX.at< float >(1, 2) = -sin(angleRad);
221  RX.at< float >(2, 1) = sin(angleRad);
222  RX.at< float >(2, 2) = cos(angleRad);
223  // now multiply
224  R = R * RX;
225  // finally, the the rodrigues back
226  Rodrigues(R, rotation);
227 }
228 
231 Board BoardDetector::detect(const cv::Mat &Image, const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters) {
232  BoardDetector BD;
233  BD.setParams(bc, cp, markerSizeMeters);
234  BD.detect(Image);
235  return BD.getDetectedBoard();
236 }
237 };
Board & getDetectedBoard()
Definition: boarddetector.h:84
cv::Mat Tvec
Definition: board.h:133
const MarkerInfo & getMarkerInfo(int id) const
Definition: board.cpp:149
int getIndexOfMarkerId(int id) const
Definition: board.cpp:139
void setParams(const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters=-1)
This class detects AR boards Version 1.2 There are two modes for board detection. First...
Definition: boarddetector.h:64
BoardConfiguration conf
Definition: board.h:131
float detect(const cv::Mat &im)
cv::Mat Rvec
Definition: board.h:133
Parameters of the camera.
This class defines a board with several markers. A Board contains several markers so that they are mo...
Definition: board.h:69


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Meißner Pascal, Qattan Mohamad
autogenerated on Mon Feb 28 2022 21:41:30