Tracker2d3d.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 // ****************************************************************************
36 // Filename: Tracker2d3d.cpp
37 // Author: Pedram Azad
38 // Date: 25.01.2008
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
48 #include "Tracker2d3d.h"
49 
50 #include "Math/FloatMatrix.h"
51 #include "Math/FloatVector.h"
52 #include "Math/LinearAlgebra.h"
53 #include "Math/Math2d.h"
54 #include "Math/Math3d.h"
56 #include "Image/ImageProcessor.h"
57 #include "Image/ByteImage.h"
58 #include "Tracking/ObjectPose.h"
59 
60 #include <stdio.h>
61 
62 
63 // ****************************************************************************
64 // This is a simple (and only partly) implementation of the algorithm
65 // described in:
66 // E. Marchand, P. Bouthemy, F. Chaumette, and V. Moreau,
67 // "Robust Real-Time Visual Tracking using a 2D-3D Model-based Approach",
68 // in Proc. of International Conference on Computer Vision (ICCV),
69 // pages 262-268, 1999.
70 // ****************************************************************************
71 
72 
73 
74 // ****************************************************************************
75 // Constructor / Destructor
76 // ****************************************************************************
77 
79 {
81  m_nPixelsDelta = 15;
82 }
83 
85 {
86 }
87 
88 
89 // ****************************************************************************
90 // Methods
91 // ****************************************************************************
92 
93 void CTracker2d3d::Init(const CCalibration *pCalibration)
94 {
95  m_pCalibration = pCalibration;
96 }
97 
98 bool CTracker2d3d::Track(const CByteImage *pEdgeImage, Vec3d *pOutlinePoints, int nOutlinePoints,
99  Mat3d &rotation, Vec3d &translation)
100 {
101  if (pEdgeImage->type != CByteImage::eGrayScale)
102  {
103  printf("error: input image must be grayscale image for CTracker2d3d::Track\n");
104  return false;
105  }
106 
107  if (m_pCalibration->GetCameraParameters().width != pEdgeImage->width ||
109  {
110  printf("error: calibration does not match image in CTracker2d3d::Track\n");
111  return false;
112  }
113 
114  const int nMaxPoints = 10000;
115 
116  Vec3d *pPointList3d = new Vec3d[nMaxPoints];
117  Vec2d *pPointList2dProjection = new Vec2d[nMaxPoints];
118  Vec2d *pPointList2d = new Vec2d[nMaxPoints];
119 
120  const unsigned char *pixels = pEdgeImage->pixels;
121  const int width = pEdgeImage->width;
122  const int height = pEdgeImage->height;
123 
124  Mat3d A;
126 
127  int nPoints;
128 
129  for (int nRun = 0;; nRun++)
130  {
131  int i;
132  nPoints = 0;
133 
134  for (i = 0; i < nOutlinePoints; i += 2)
135  {
136  Vec3d p1, p2;
137  Math3d::SetVec(p1, pOutlinePoints[i]);
138  Math3d::SetVec(p2, pOutlinePoints[i + 1]);
139 
140  Vec2d p1_, p2_;
141  {
142  Vec3d temp;
143 
144  Math3d::MulMatVec(rotation, p1, translation, temp);
145  m_pCalibration->WorldToImageCoordinates(temp, p1_, false);
146 
147  Math3d::MulMatVec(rotation, p2, translation, temp);
148  m_pCalibration->WorldToImageCoordinates(temp, p2_, false);
149 
150  Math3d::SetVec(temp, p1_.x, p1_.y, 1);
151  Math3d::MulMatVec(A, temp, temp);
152  p1_.x = temp.x / temp.z;
153  p1_.y = temp.y / temp.z;
154 
155  Math3d::SetVec(temp, p2_.x, p2_.y, 1);
156  Math3d::MulMatVec(A, temp, temp);
157  p2_.x = temp.x / temp.z;
158  p2_.y = temp.y / temp.z;
159  }
160 
161  const float length_ = Math2d::Distance(p1_, p2_);
162 
163  const int nControlPoints = int(length_ / m_nPixelsDelta) - 1;
164  if (nControlPoints < 1 || nControlPoints > 10000)
165  continue;
166 
167  Vec3d u;
168  Math3d::SubtractVecVec(p2, p1, u);
169  const float delta_length = Math3d::Length(u) / nControlPoints;
171  Math3d::MulVecScalar(u, delta_length, u);
172 
173  Vec2d u_ = { p1_.x - p2_.x, p1_.y - p2_.y };
175 
176  // calculate normal vector
177  Vec2d n = { p1_.y - p2_.y, p2_.x - p1_.x };
179 
180  Vec3d p;
181  Math3d::SetVec(p, p1);
182 
183  for (int j = 0; j < nControlPoints; j++)
184  {
185  Math3d::AddToVec(p, u);
186 
187  Vec2d p_;
188  {
189  Vec3d temp;
190  Vec2d temp2;
191 
192  Math3d::MulMatVec(rotation, p, translation, temp);
193  m_pCalibration->WorldToImageCoordinates(temp, temp2, false);
194 
195  Vec3d temp3 = { temp2.x, temp2.y, 1 };
196 
197  Math3d::MulMatVec(A, temp3, temp3);
198 
199  p_.x = temp3.x / temp3.z;
200  p_.y = temp3.y / temp3.z;
201  }
202 
203  // search in perpendicular direction
204  Vec2d pn1, pn2;
205  int k, l = -1;
206  bool bPlus = true;
207 
208  Math2d::SetVec(pn1, p_);
209  Math2d::SetVec(pn2, p_);
210  for (k = 0; k < m_nPixelsSearchDistance; k++)
211  {
212  int x, y;
213 
214  // can be optimized: checking boundaries within loop is slow but convenient
215  x = int(pn1.x + 0.5f);
216  y = int(pn1.y + 0.5f);
217  if (x > 0 && x < width && y > 0 && y < height && pixels[y * width + x])
218  {
219  l = k;
220  bPlus = true;
221  break;
222  }
223 
224  x = int(pn2.x + 0.5f);
225  y = int(pn2.y + 0.5f);
226  if (x > 0 && x < width && y > 0 && y < height && pixels[y * width + x])
227  {
228  l = k;
229  bPlus = false;
230  break;
231  }
232 
233  Math2d::AddToVec(pn1, n);
234  Math2d::SubtractFromVec(pn2, n);
235  }
236 
237  if (l != -1)
238  {
239  Math3d::SetVec(pPointList3d[nPoints], p);
240  Math2d::SetVec(pPointList2dProjection[nPoints], p_);
241 
242  if (bPlus)
243  {
244  Math2d::SetVec(pPointList2d[nPoints], pn1);
245  }
246  else
247  {
248  Math2d::SetVec(pPointList2d[nPoints], pn2);
249  }
250 
251  if (++nPoints == nMaxPoints)
252  break;
253  }
254  }
255  }
256 
257  if (nPoints < 6)
258  return false;
259 
260  if (nRun == 4)
261  {
262  // end after five runs
263  break;
264  }
265 
266  Mat3d A_;
267  LinearAlgebra::DetermineAffineTransformation(pPointList2dProjection, pPointList2d, nPoints, A_);
268  Math3d::MulMatMat(A_, A, A);
269  }
270 
271  CObjectPose objectPose(pPointList3d, nPoints);
272  objectPose.EstimatePose(pPointList2d, rotation, translation, m_pCalibration);
273 
274  delete [] pPointList3d;
275  delete [] pPointList2d;
276  delete [] pPointList2dProjection;
277 
278  return true;
279 }
Implementation of an alternative to the POSIT algorithm.
Definition: ObjectPose.h:74
void Init(const CCalibration *pCalibration)
Definition: Tracker2d3d.cpp:93
float y
Definition: Math2d.h:84
int width
The width of the image in pixels.
Definition: ByteImage.h:257
GLenum GLsizei n
Definition: glext.h:4209
int m_nPixelsSearchDistance
Definition: Tracker2d3d.h:102
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
float x
Definition: Math3d.h:75
Data structure for the representation of 8-bit grayscale images and 24-bit RGB (or HSV) color images ...
Definition: ByteImage.h:80
float Length(const Vec3d &vec)
Definition: Math3d.cpp:585
float z
Definition: Math3d.h:75
unsigned char * pixels
The pointer to the the pixels.
Definition: ByteImage.h:283
GLenum GLint x
Definition: glext.h:3125
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
Definition: Calibration.h:268
float Distance(const Vec2d &vector1, const Vec2d &vector2)
Definition: Math2d.cpp:181
float x
Definition: Math2d.h:84
bool DetermineAffineTransformation(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD=false)
Determines an affine transformation based on a set of 2d-2d point correspondences.
bool EstimatePose(const Vec2d *pImagePoints, Mat3d &rotationMatrix, Vec3d &translationVector, const CCalibration *pCalibration, int nMaxIterations=350)
Definition: ObjectPose.cpp:121
void SetVec(Vec3d &vec, float x, float y, float z)
Definition: Math3d.cpp:243
float y
Definition: Math3d.h:75
void MulVecScalar(const Vec3d &vec, float scalar, Vec3d &result)
Definition: Math3d.cpp:502
bool Track(const CByteImage *pEdgeImage, Vec3d *pOutlinePoints, int nOutlinePoints, Mat3d &rotation, Vec3d &translation)
Definition: Tracker2d3d.cpp:98
void MulMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
Definition: Math3d.cpp:444
void AddToVec(Vec3d &vec, const Vec3d &vectorToAdd)
Definition: Math3d.cpp:481
void MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
Definition: Math3d.cpp:422
void NormalizeVec(Vec2d &vec)
Definition: Math2d.cpp:160
GLint GLint GLsizei GLsizei GLsizei GLint GLenum GLenum const GLvoid * pixels
Definition: glext.h:3154
int height
The height of the image in pixels.
Definition: ByteImage.h:264
GLenum GLsizei width
Definition: glext.h:3122
GLenum GLsizei GLsizei height
Definition: glext.h:3132
void AddToVec(Vec2d &vec, const Vec2d &vectorToAdd)
Definition: Math2d.cpp:81
ImageType type
The type of the image.
Definition: ByteImage.h:292
int m_nPixelsDelta
Definition: Tracker2d3d.h:101
void NormalizeVec(Vec3d &vec)
Definition: Math3d.cpp:573
const Mat3d unit_mat
Definition: Math3d.cpp:60
GLenum GLint GLint y
Definition: glext.h:3125
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
void SubtractVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:522
const CCalibration * m_pCalibration
Definition: Tracker2d3d.h:99
void SetVec(Vec2d &vec, float x, float y)
Definition: Math2d.cpp:68
void SubtractFromVec(Vec2d &vec, const Vec2d &vectorToSubtract)
Definition: Math2d.cpp:87
GLfloat GLfloat p
Definition: glext.h:5178
void SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
Definition: Math3d.cpp:257
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
Camera model parameters and functions for a single camera.
Definition: Calibration.h:125
void WorldToImageCoordinates(const Vec3d &worldPoint, Vec2d &imagePoint, bool bUseDistortionParameters=true) const
Transforms 3D world coordinates to 2D image coordinates.


asr_ivt
Author(s): Allgeyer Tobias, Hutmacher Robin, Kleinert Daniel, Meißner Pascal, Scholz Jonas, Stöckle Patrick
autogenerated on Mon Dec 2 2019 03:47:28