Main MRPT website > C++ reference for MRPT 1.5.5
posit.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 #include <iostream>
12 #include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
13 #include <Eigen/Dense>
14 #include <Eigen/SVD>
15 
16 #include "posit.h"
17 
18 
19 mrpt::vision::pnp::posit::posit(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd camera_intrinsic_, int n0)
20 {
21  obj_pts=obj_pts_;
22  img_pts=img_pts_.block(0,0,n0,2);
23  cam_intrinsic=camera_intrinsic_;
24  R=Eigen::MatrixXd::Identity(3,3);
25  t=Eigen::VectorXd::Zero(3);
26  f=(cam_intrinsic(0,0)+cam_intrinsic(1,1))/2;
27 
28  obj_matrix=(obj_pts.transpose() * obj_pts).inverse() * obj_pts.transpose();
29 
30  n=n0;
31 
32  obj_vecs=Eigen::MatrixXd::Zero(n0,3);
33 
34  for(int i=0;i<n;i++)
35  obj_vecs.row(i)=obj_pts.row(i)-obj_pts.row(0);
36 
37  img_vecs = Eigen::MatrixXd::Zero(n0,2);
39 
40  epsilons=Eigen::VectorXd::Zero(n);
41 }
42 
44 {
45  Eigen::Vector3d I0, J0 , r1, r2, r3;
46  double I0_norm, J0_norm;
47 
48  int i;
49  double scale;
50 
51  for(i=0;i<3;i++)
52  {
53  I0(i)=obj_matrix.row(i).dot(img_vecs.col(0));
54  J0(i)=obj_matrix.row(i).dot(img_vecs.col(1));
55  }
56 
57 
58  I0_norm=I0.norm();
59  J0_norm=J0.norm();
60 
61  scale=(I0_norm + J0_norm)/2;
62 
63  /*Computing TRANSLATION */
64  t(0)=img_pts(0,0)/scale;
65  t(1)=img_pts(0,1)/scale;
66  t(2)=f/scale;
67 
68  /* Computing ROTATION */
69  r1=I0/I0_norm;
70  r2=J0/J0_norm;
71  r3=r1.cross(r2);
72 
73  R.row(0)=r1;
74  R.row(1)=r2;
75  R.row(2)=r3;
76 }
77 
78 /**
79 Iterate over results obtained by the POS function;
80 see paper "Model-Based Object Pose in 25 Lines of Code", IJCV 15, pp. 123-141, 1995.
81 */
82 bool mrpt::vision::pnp::posit::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
83 {
84  Eigen::FullPivLU<Eigen::MatrixXd> lu(obj_pts);
85  if(lu.rank()<3)
86  return false;
87 
88  int i, iCount;
89  long imageDiff=1000;
90 
91  for(iCount=0; iCount<LOOP_MAX_COUNT;iCount++)
92  {
93  if(iCount==0)
94  {
95  for(i=0;i<img_vecs.rows();i++)
96  img_vecs.row(i)=img_pts.row(i)-img_pts.row(0);
97  }
98 
99  else
100  {
101  // Compute new image vectors
102  epsilons.setZero();
103  for(i=0; i<n; i++)
104  {
105  epsilons(i)+=obj_vecs.row(i).dot(R.row(2));
106  }
107  epsilons/=t(2);
108 
109  // Corrected image vectors
110  for(i=0; i<n; i++)
111  {
112  img_vecs.row(i)= img_pts.row(i) * (1+epsilons(i)) -img_pts.row(0);
113  }
114 
115  imageDiff=this->get_img_diff();
116 
117  }
118 
119  img_vecs_old=img_vecs;
120 
121  this->POS();
122 
123  if(iCount>0 && imageDiff==0)
124  break;
125 
126  if(iCount==LOOP_MAX_COUNT)
127  {
128  std::cout<<"Solution Not converged"<<std::endl<<std::endl;
129  break;
130  }
131 
132  }
133  R_=R;
134  t_=t;
135 
136  return true;
137 }
138 
140 {
141  int i, j;
142  long sumOfDiffs = 0;
143 
144  for (i=0;i<n;i++){
145  for (j=0;j<2;j++){
146  sumOfDiffs += std::abs(floor(0.5+img_vecs(i,j))-floor(0.5+img_vecs_old(i,j)));
147  }
148  }
149  return sumOfDiffs;
150 
151 }
152 
153 
154 
155 
156 
Eigen::MatrixXd obj_pts
Definition: posit.h:39
GLdouble GLdouble t
Definition: glext.h:3610
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Computes pose using iterative computation of POS()
Definition: posit.cpp:82
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
#define LOOP_MAX_COUNT
Definition: posit.h:16
GLenum GLsizei n
Definition: glext.h:4618
Eigen::VectorXd epsilons
Focal Length from camera intrinsic matrix.
Definition: posit.h:45
Eigen::MatrixXd obj_vecs
Translation Vector.
Definition: posit.h:51
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
Eigen::MatrixXd R
Number of 2d/3d correspondences.
Definition: posit.h:48
int n
Co-efficients used for scaling.
Definition: posit.h:46
const float R
posit(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd camera_intrinsic_, int n)
Used to store img_vecs from previous iteration.
Definition: posit.cpp:19
Eigen::MatrixXd img_pts
Object Points in Camera Co-ordinate system.
Definition: posit.h:40
long get_img_diff()
Function to check for convergence.
Definition: posit.cpp:139
Eigen::MatrixXd img_vecs
Object Points relative to 1st object point.
Definition: posit.h:52
double f
Pseudo-Inverse of Object Points matrix.
Definition: posit.h:44
Pose from Orthogonality and Scaling (POSIT) - Eigen Implementation.
Eigen::MatrixXd obj_matrix
Camera Intrinsic matrix.
Definition: posit.h:42
Eigen::MatrixXd cam_intrinsic
Image Points in pixels.
Definition: posit.h:41
Eigen::MatrixXd img_vecs_old
Image Points relative to 1st image point.
Definition: posit.h:53
void POS()
Function used to compute pose from orthogonality.
Definition: posit.cpp:43



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019