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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 31e853f50 Thu Nov 21 23:57:32 2019 +0100 at vie nov 22 00:00:11 CET 2019