posetracker.h
Go to the documentation of this file.
1 /*****************************
2 Copyright 2016 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 #ifndef ARUCO_POSETRACKER
29 #define ARUCO_POSETRACKER
30 #include "exports.h"
31 #include <opencv2/core/core.hpp>
32 #include "marker.h"
33 #include "markermap.h"
34 #include "cameraparameters.h"
35 #include <map>
36 namespace aruco{
37 
58  public:
67  bool estimatePose( Marker &m,const CameraParameters &cam_params,float markerSize,float minErrorRatio=4);
68 
69  //returns the 4x4 transform matrix. Returns an empty matrix if last call to estimatePose returned false
70  cv::Mat getRTMatrix()const;
71  //return the rotation vector. Returns an empty matrix if last call to estimatePose returned false
72  const cv::Mat getRvec()const{return _rvec;}
73  //return the translation vector. Returns an empty matrix if last call to estimatePose returned false
74  const cv::Mat getTvec()const{return _tvec;}
75 
76  private:
77  cv::Mat _rvec,_tvec;//current poses
78  double solve_pnp(const std::vector<cv::Point3f> & p3d,const std::vector<cv::Point2f> & p2d,const cv::Mat &cam_matrix,const cv::Mat &dist,cv::Mat &r_io,cv::Mat &t_io);
79 
80 };
85 
86 public:
88  //Sets the parameters required for operation
89  //If the msconf has data expressed in meters, then the markerSize parameter is not required. If it is in pixels, the markersize will be used to
90  //transform to meters
91  //Throws exception if wrong configuraiton
92  void setParams(const CameraParameters &cam_params,const MarkerMap &msconf, float markerSize=-1)throw(cv::Exception);
93  //indicates if the call to setParams has been successfull and this object is ready to call estimatePose
94  bool isValid()const{return _isValid;}
95  //estimates camera pose wrt the markermap
96  //returns true if pose has been obtained and false otherwise
97  bool estimatePose(const vector<Marker> &v_m);
98 
99  //returns the 4x4 transform matrix. Returns an empty matrix if last call to estimatePose returned false
100  cv::Mat getRTMatrix()const;
101  //return the rotation vector. Returns an empty matrix if last call to estimatePose returned false
102  const cv::Mat getRvec()const{return _rvec;}
103  //return the translation vector. Returns an empty matrix if last call to estimatePose returned false
104  const cv::Mat getTvec()const{return _tvec;}
105 private:
106 
107  cv::Mat _rvec,_tvec;//current poses
110  std::map<int,Marker3DInfo> _map_mm;
111  bool _isValid;
112 };
113 
114 };
115 
116 #endif
117 
#define ARUCO_EXPORTS
Definition: exports.h:42
const cv::Mat getTvec() const
Definition: posetracker.h:104
const cv::Mat getRvec() const
Definition: posetracker.h:102
std::map< int, Marker3DInfo > _map_mm
Definition: posetracker.h:110
cv::Mat getRTMatrix(const cv::Mat &R_, const cv::Mat &T_, int forceType)
Definition: ippe.cpp:11
This class represents a marker. It is a vector of the fours corners ot the marker.
Definition: marker.h:42
const cv::Mat getRvec() const
Definition: posetracker.h:72
Parameters of the camera.
const cv::Mat getTvec() const
Definition: posetracker.h:74
This class defines a set of markers whose locations are attached to a common reference system...
Definition: markermap.h:71
aruco::CameraParameters _cam_params
Definition: posetracker.h:108


tuw_aruco
Author(s): Lukas Pfeifhofer
autogenerated on Mon Jun 10 2019 15:40:55