RPP.h
Go to the documentation of this file.
1 /*
2 This is a C++ port using OpenCV of the Robust Pose Estimation from a Planar Target algorithm by Gerald Schweighofer and Axel Pinz.
3 
4 It is a line by line port of the Matlab code at
5 
6 http://www.emt.tugraz.at/~pinz/code/
7 
8 I have no idea what their license is, if any. I'll make my code BSD for now unless I later find they have a more restrictive one.
9 */
10 
11 /*
12 Copyright 2011 Nghia Ho. All rights reserved.
13 
14 Redistribution and use in source and binary forms, with or without modification, are
15 permitted provided that the following conditions are met:
16 
17  1. Redistributions of source code must retain the above copyright notice, this list of
18  conditions and the following disclaimer.
19 
20  2. Redistributions in binary form must reproduce the above copyright notice, this list
21  of conditions and the following disclaimer in the documentation and/or other materials
22  provided with the distribution.
23 
24 THIS SOFTWARE IS PROVIDED BY NGHIA HO ''AS IS'' AND ANY EXPRESS OR IMPLIED
25 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
26 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL NGHIA HO OR
27 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
28 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
30 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
31 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
32 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 The views and conclusions contained in the software and documentation are those of the
35 authors and should not be interpreted as representing official policies, either expressed
36 or implied, of Nghia Ho.
37 */
38 
39 #ifndef __RPP_H__
40 #define __RPP_H__
41 
42 #include <opencv2/core/core.hpp>
43 #include <vector>
44 #include "Rpoly.h"
45 
46 namespace RPP
47 {
48 
49 class Quaternion
50 {
51 public:
52 
53  cv::Vec3d vector;
54  double scalar;
55 
56  Quaternion(){}
57  Quaternion(const cv::Vec3d &v, double s) {
58  vector = v;
59  scalar = s;
60  }
61 };
62 
63 class Solution
64 {
65 public:
66  Solution()
67  {
68  R = cv::Mat::zeros(3,3,CV_64F);
69  t = cv::Mat::zeros(3,1,CV_64F);
70  }
71 
72  Solution(const Solution &s) {
73  R = s.R.clone();
74  t = s.t.clone();
75  E = s.E;
76  bl = s.bl;
77  at = s.at;
78  obj_err = s.obj_err;
79  img_err = s.img_err;
80  }
81 
82 
83  cv::Mat R;
84  cv::Mat t;
85  double E;
86  double bl;
87  double at;
88  double obj_err;
89  double img_err;
90 };
91 
92 bool Rpp(const cv::Mat &model_3D, const cv::Mat &iprts,
93  cv::Mat &Rlu, cv::Mat &tlu, int &it1, double &obj_err1, double &img_err1, std::vector<Solution>&sol);
94 
95 void ObjPose(const cv::Mat P, cv::Mat Qp, cv::Mat initR,
96  cv::Mat &R, cv::Mat &t, int &it, double &obj_err, double &img_err);
97 
98 void AbsKernel(cv::Mat P, cv::Mat Q, const std::vector <cv::Mat> &F, const cv::Mat &G,
99  cv::Mat &R, cv::Mat &t, cv::Mat &Qout, double &err2);
100 
101 cv::Mat EstimateT(const cv::Mat &R, const cv::Mat &G, const std::vector <cv::Mat> &F, const cv::Mat &P);
102 cv::Mat NormRv(const cv::Mat &R);
103 cv::Mat NormRv(const cv::Vec3d &V);
104 
105 Quaternion Quaternion_byAngleAndVector(double q_angle, const cv::Vec3d &q_vector);
106 cv::Mat quat2mat(const Quaternion &Q);
107 cv::Mat GetRotationbyVector(const cv::Vec3d &v1, const cv::Vec3d &v2);
108 
109 bool Get2ndPose_Exact(const cv::Mat &v, const cv::Mat &P, const cv::Mat &R, const cv::Mat &t, std::vector <Solution> &ret);
110 
111 bool GetRfor2ndPose_V_Exact(const cv::Mat &v, const cv::Mat &P, const cv::Mat &R, const cv::Mat &t, std::vector <Solution> &ret);
112 
113 void GetRotationY_wrtT(const cv::Mat &v, const cv::Mat &P, const cv::Mat &t, const cv::Mat &Rz,
114  std::vector <double> &al, cv::Mat &tnew, std::vector <double> &at);
115 
116 
117 bool DecomposeR(const cv::Mat &R, cv::Mat &Rz2, cv::Mat &ret);
118 
119 cv::Mat RpyMat(const cv::Vec3d &angs);
120 bool RpyAng(const cv::Mat &R, cv::Vec3d &ret); // returns roll,pitch,yaw
121 cv::Mat Mean(const cv::Mat &m);
122 cv::Mat Sum(const cv::Mat &m, int dim=1);
123 cv::Mat Mul(const cv::Mat &a, const cv::Mat &b);
124 cv::Mat Sq(const cv::Mat &m);
125 cv::Mat Xform(const cv::Mat &P, const cv::Mat &R, const cv::Mat &t);
126 double Norm(const cv::Mat &m);
127 
128 void Print(const cv::Mat &m);
129 void Print(const Quaternion &q);
130 
131 // Makes homogenous points
132 cv::Mat Point2Mat(const std::vector <cv::Point3d> &pts);
133 cv::Mat Point2Mat(const std::vector <cv::Point2d> &pts);
134 cv::Mat Vec2Mat(const cv::Vec3d &v);
135 
136 bool RpyAng_X(const cv::Mat &R, cv::Vec3d &ret);
137 
138 
139 inline Quaternion Quaternion_byVectorAndScalar(const cv::Vec3d &vector, double scalar)
140 {
141  return Quaternion(vector, scalar);
142 }
143 
144 inline Quaternion Quaternion_multiplyByScalar(const Quaternion &q, double scalar)
145 {
146  Quaternion ret;
147 
148  ret.vector[0] = q.vector[0]*scalar;
149  ret.vector[1] = q.vector[1]*scalar;
150  ret.vector[2] = q.vector[2]*scalar;
151 
152  ret.scalar = q.scalar*scalar;
153 
154  return ret;
155 }
156 
157 inline double Quaternion_Norm(const Quaternion &Q)
158 {
159  return sqrt(Q.vector[0]*Q.vector[0] + Q.vector[1]*Q.vector[1] + Q.vector[2]*Q.vector[2] + Q.scalar*Q.scalar);
160 }
161 
162 inline int sign(double x)
163 {
164  if(x < 0)
165  return -1;
166 
167  if(x > 0)
168  return 1;
169 
170  return 0;
171 }
172 
173 } // End namespace
174 #endif
RPP::Solution::R
cv::Mat R
Definition: RPP.h:83
RPP::Mul
cv::Mat Mul(const cv::Mat &a, const cv::Mat &b)
RPP::Solution::E
double E
Definition: RPP.h:85
RPP::Get2ndPose_Exact
bool Get2ndPose_Exact(const cv::Mat &v, const cv::Mat &P, const cv::Mat &R, const cv::Mat &t, std::vector< Solution > &ret)
RPP::Solution::at
double at
Definition: RPP.h:87
RPP::Sq
cv::Mat Sq(const cv::Mat &m)
RPP::Vec2Mat
cv::Mat Vec2Mat(const cv::Vec3d &v)
RPP::Rpp
bool Rpp(const cv::Mat &model_3D, const cv::Mat &iprts, cv::Mat &Rlu, cv::Mat &tlu, int &it1, double &obj_err1, double &img_err1, std::vector< Solution > &sol)
RPP::RpyAng
bool RpyAng(const cv::Mat &R, cv::Vec3d &ret)
RPP::Quaternion_byAngleAndVector
Quaternion Quaternion_byAngleAndVector(double q_angle, const cv::Vec3d &q_vector)
RPP::Sum
cv::Mat Sum(const cv::Mat &m, int dim=1)
RPP
Definition: RPP.h:46
RPP::Quaternion_Norm
double Quaternion_Norm(const Quaternion &Q)
Definition: RPP.h:157
RPP::Print
void Print(const cv::Mat &m)
RPP::Quaternion::Quaternion
Quaternion()
Definition: RPP.h:56
RPP::NormRv
cv::Mat NormRv(const cv::Mat &R)
RPP::sign
int sign(double x)
Definition: RPP.h:162
RPP::GetRotationbyVector
cv::Mat GetRotationbyVector(const cv::Vec3d &v1, const cv::Vec3d &v2)
RPP::EstimateT
cv::Mat EstimateT(const cv::Mat &R, const cv::Mat &G, const std::vector< cv::Mat > &F, const cv::Mat &P)
RPP::Solution::obj_err
double obj_err
Definition: RPP.h:88
RPP::GetRotationY_wrtT
void GetRotationY_wrtT(const cv::Mat &v, const cv::Mat &P, const cv::Mat &t, const cv::Mat &Rz, std::vector< double > &al, cv::Mat &tnew, std::vector< double > &at)
RPP::Norm
double Norm(const cv::Mat &m)
RPP::RpyAng_X
bool RpyAng_X(const cv::Mat &R, cv::Vec3d &ret)
RPP::AbsKernel
void AbsKernel(cv::Mat P, cv::Mat Q, const std::vector< cv::Mat > &F, const cv::Mat &G, cv::Mat &R, cv::Mat &t, cv::Mat &Qout, double &err2)
RPP::GetRfor2ndPose_V_Exact
bool GetRfor2ndPose_V_Exact(const cv::Mat &v, const cv::Mat &P, const cv::Mat &R, const cv::Mat &t, std::vector< Solution > &ret)
RPP::quat2mat
cv::Mat quat2mat(const Quaternion &Q)
Definition: RPP.cpp:401
RPP::Xform
cv::Mat Xform(const cv::Mat &P, const cv::Mat &R, const cv::Mat &t)
RPP::ObjPose
void ObjPose(const cv::Mat P, cv::Mat Qp, cv::Mat initR, cv::Mat &R, cv::Mat &t, int &it, double &obj_err, double &img_err)
RPP::Quaternion::Quaternion
Quaternion(const cv::Vec3d &v, double s)
Definition: RPP.h:57
RPP::Solution::t
cv::Mat t
Definition: RPP.h:84
RPP::Point2Mat
cv::Mat Point2Mat(const std::vector< cv::Point3d > &pts)
RPP::Quaternion::vector
cv::Vec3d vector
Definition: RPP.h:53
RPP::RpyMat
cv::Mat RpyMat(const cv::Vec3d &angs)
RPP::Solution::img_err
double img_err
Definition: RPP.h:89
RPP::Quaternion::scalar
double scalar
Definition: RPP.h:54
RPP::Quaternion_byVectorAndScalar
Quaternion Quaternion_byVectorAndScalar(const cv::Vec3d &vector, double scalar)
Definition: RPP.h:139
RPP::Solution::bl
double bl
Definition: RPP.h:86
RPP::Mean
cv::Mat Mean(const cv::Mat &m)
RPP::Quaternion_multiplyByScalar
Quaternion Quaternion_multiplyByScalar(const Quaternion &q, double scalar)
Definition: RPP.h:144
RPP::Solution::Solution
Solution(const Solution &s)
Definition: RPP.h:72
RPP::Solution::Solution
Solution()
Definition: RPP.h:66
RPP::DecomposeR
bool DecomposeR(const cv::Mat &R, cv::Mat &Rz2, cv::Mat &ret)

fiducial_pose
Author(s): Jim Vaughan
autogenerated on Thu Dec 28 2017 04:06:58