Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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/math/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 
19  Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_,
20  Eigen::MatrixXd camera_intrinsic_, int n0)
21 {
22  obj_pts = obj_pts_;
23  img_pts = img_pts_.block(0, 0, n0, 2);
24  cam_intrinsic = camera_intrinsic_;
25  R = Eigen::MatrixXd::Identity(3, 3);
26  t = Eigen::VectorXd::Zero(3);
27  f = (cam_intrinsic(0, 0) + cam_intrinsic(1, 1)) / 2;
28 
29  obj_matrix =
30  (obj_pts.transpose() * obj_pts).inverse() * obj_pts.transpose();
31 
32  n = n0;
33 
34  obj_vecs = Eigen::MatrixXd::Zero(n0, 3);
35 
36  for (int i = 0; i < n; i++)
37  obj_vecs.row(i) = obj_pts.row(i) - obj_pts.row(0);
38 
39  img_vecs = Eigen::MatrixXd::Zero(n0, 2);
41 
42  epsilons = Eigen::VectorXd::Zero(n);
43 }
44 
46 {
47  Eigen::Vector3d I0, J0, r1, r2, r3;
48  double I0_norm, J0_norm;
49 
50  int i;
51  double scale;
52 
53  for (i = 0; i < 3; i++)
54  {
55  I0(i) = obj_matrix.row(i).dot(img_vecs.col(0));
56  J0(i) = obj_matrix.row(i).dot(img_vecs.col(1));
57  }
58 
59  I0_norm = I0.norm();
60  J0_norm = J0.norm();
61 
62  scale = (I0_norm + J0_norm) / 2;
63 
64  /*Computing TRANSLATION */
65  t(0) = img_pts(0, 0) / scale;
66  t(1) = img_pts(0, 1) / scale;
67  t(2) = f / scale;
68 
69  /* Computing ROTATION */
70  r1 = I0 / I0_norm;
71  r2 = J0 / J0_norm;
72  r3 = r1.cross(r2);
73 
74  R.row(0) = r1;
75  R.row(1) = r2;
76  R.row(2) = r3;
77 }
78 
79 /**
80 Iterate over results obtained by the POS function;
81 see paper "Model-Based Object Pose in 25 Lines of Code", IJCV 15, pp. 123-141,
82 1995.
83 */
85  Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
86 {
87  Eigen::FullPivLU<Eigen::MatrixXd> lu(obj_pts);
88  if (lu.rank() < 3) return false;
89 
90  int i, iCount;
91  long imageDiff = 1000;
92 
93  for (iCount = 0; iCount < LOOP_MAX_COUNT; iCount++)
94  {
95  if (iCount == 0)
96  {
97  for (i = 0; i < img_vecs.rows(); i++)
98  img_vecs.row(i) = img_pts.row(i) - img_pts.row(0);
99  }
100 
101  else
102  {
103  // Compute new image vectors
104  epsilons.setZero();
105  for (i = 0; i < n; i++)
106  {
107  epsilons(i) += obj_vecs.row(i).dot(R.row(2));
108  }
109  epsilons /= t(2);
110 
111  // Corrected image vectors
112  for (i = 0; i < n; i++)
113  {
114  img_vecs.row(i) =
115  img_pts.row(i) * (1 + epsilons(i)) - img_pts.row(0);
116  }
117 
118  imageDiff = this->get_img_diff();
119  }
120 
121  img_vecs_old = img_vecs;
122 
123  this->POS();
124 
125  if (iCount > 0 && imageDiff == 0) break;
126 
127  if (iCount == LOOP_MAX_COUNT)
128  {
129  std::cout << "Solution Not converged" << std::endl << std::endl;
130  break;
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  {
146  for (j = 0; j < 2; j++)
147  {
148  sumOfDiffs += std::abs(
149  floor(0.5 + img_vecs(i, j)) - floor(0.5 + img_vecs_old(i, j)));
150  }
151  }
152  return sumOfDiffs;
153 }
Eigen::MatrixXd obj_pts
Definition: posit.h:38
GLdouble GLdouble t
Definition: glext.h:3689
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Computes pose using iterative computation of POS()
Definition: posit.cpp:84
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6502
#define LOOP_MAX_COUNT
Definition: posit.h:16
GLenum GLsizei n
Definition: glext.h:5074
Eigen::VectorXd epsilons
Focal Length from camera intrinsic matrix.
Definition: posit.h:44
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
Eigen::MatrixXd obj_vecs
Translation Vector.
Definition: posit.h:50
Eigen::MatrixXd R
Number of 2d/3d correspondences.
Definition: posit.h:47
int n
Co-efficients used for scaling.
Definition: posit.h:45
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:18
Eigen::MatrixXd img_pts
Object Points in Camera Co-ordinate system.
Definition: posit.h:39
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:51
double f
Pseudo-Inverse of Object Points matrix.
Definition: posit.h:43
Pose from Orthogonality and Scaling (POSIT) - Eigen Implementation.
Eigen::MatrixXd obj_matrix
Camera Intrinsic matrix.
Definition: posit.h:41
Eigen::MatrixXd cam_intrinsic
Image Points in pixels.
Definition: posit.h:40
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:45



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019