Visual Servoing Platform version 3.5.0
vpFeatureVanishingPoint.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
33 *
34 * Authors:
35 * Odile Bourquardez
36 *
37 *****************************************************************************/
38
43#include <visp3/visual_features/vpBasicFeature.h>
44#include <visp3/visual_features/vpFeatureVanishingPoint.h>
45
46// Exception
47#include <visp3/core/vpException.h>
48#include <visp3/visual_features/vpFeatureException.h>
49
50// Debug trace
51#include <visp3/core/vpDebug.h>
52
53// math
54#include <visp3/core/vpMath.h>
55
56#include <visp3/core/vpFeatureDisplay.h>
57
62{
63 // Feature dimension
64 dim_s = 5;
65 nbParameters = 5;
66 m_select = 0;
67
68 // memory allocation
69 s.resize(dim_s);
70 if (flags == NULL)
71 flags = new bool[nbParameters];
72 for (unsigned int i = 0; i < nbParameters; i++)
73 flags[i] = false;
74}
75
78
81{
82 s[0] = x;
83 flags[0] = true;
84 m_select |= selectX();
85}
86
88double vpFeatureVanishingPoint::get_x() const { return s[0]; }
89
92{
93 s[1] = y;
94 flags[1] = true;
95 m_select |= selectY();
96}
97
99double vpFeatureVanishingPoint::get_y() const { return s[1]; }
100
102void vpFeatureVanishingPoint::set_xy(double x, double y)
103{
104 set_x(x);
105 set_y(y);
106 m_select = selectX() | selectY();
107}
108
111{
112 s[2] = one_over_rho;
113 flags[2] = true;
115}
116
118void vpFeatureVanishingPoint::setAtanOneOverRho(double atan_one_over_rho)
119{
120 s[3] = atan_one_over_rho;
121 flags[3] = true;
123}
124
126double vpFeatureVanishingPoint::getOneOverRho() const { return s[2]; }
127
130
133{
134 s[4] = alpha;
135 flags[4] = true;
137}
138
140double vpFeatureVanishingPoint::getAlpha() const { return s[4]; }
141
151{
152 vpMatrix L;
153
154 L.resize(0, 6);
155
157 for (unsigned int i = 0; i < nbParameters; i++) {
158 if (flags[i] == false) {
159 switch (i) {
160 case 0:
161 vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
162 break;
163 case 1:
164 vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
165 break;
166 case 2:
167 vpTRACE("Warning !!! The interaction matrix is computed but 1/rho was not set yet");
168 break;
169 case 3:
170 vpTRACE("Warning !!! The interaction matrix is computed but atan(1/rho) was not set yet");
171 break;
172 case 4:
173 vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
174 break;
175 default:
176 vpTRACE("Problem during the reading of the variable flags");
177 }
178 }
179 }
180 resetFlags();
181 }
182
183 if (vpFeatureVanishingPoint::selectX() & select) {
184 double x = get_x();
185 double y = get_y();
186 vpMatrix Lx(1, 6);
187 Lx = 0;
188
189 Lx[0][0] = 0.;
190 Lx[0][1] = 0.;
191 Lx[0][2] = 0.;
192 Lx[0][3] = x * y;
193 Lx[0][4] = -(1 + x * x);
194 Lx[0][5] = y;
195
196 L = vpMatrix::stack(L, Lx);
197 }
198
199 if (vpFeatureVanishingPoint::selectY() & select) {
200 double x = get_x();
201 double y = get_y();
202 vpMatrix Ly(1, 6);
203 Ly = 0;
204
205 Ly[0][0] = 0;
206 Ly[0][1] = 0.;
207 Ly[0][2] = 0.;
208 Ly[0][3] = 1 + y * y;
209 Ly[0][4] = -x * y;
210 Ly[0][5] = -x;
211
212 L = vpMatrix::stack(L, Ly);
213 }
214
216 double one_over_rho = getOneOverRho();
217 double alpha = getAlpha();
218 vpMatrix Lone_over_rho(1, 6);
219 double rho2 = 1. + one_over_rho * one_over_rho;
220
221 Lone_over_rho[0][0] = 0.;
222 Lone_over_rho[0][1] = 0.;
223 Lone_over_rho[0][2] = 0.;
224 Lone_over_rho[0][3] = - rho2 * sin(alpha);
225 Lone_over_rho[0][4] = rho2 * cos(alpha);
226 Lone_over_rho[0][5] = 0.;
227
228 L = vpMatrix::stack(L, Lone_over_rho);
229 }
230
232 double alpha = getAlpha();
233 vpMatrix Latan_one_over_rho(1, 6);
234
235 Latan_one_over_rho[0][0] = 0.;
236 Latan_one_over_rho[0][1] = 0.;
237 Latan_one_over_rho[0][2] = 0.;
238 Latan_one_over_rho[0][3] = - sin(alpha);
239 Latan_one_over_rho[0][4] = cos(alpha);
240 Latan_one_over_rho[0][5] = 0.;
241
242 L = vpMatrix::stack(L, Latan_one_over_rho);
243 }
244
246 double one_over_rho = getOneOverRho();
247 double alpha = getAlpha();
248 vpMatrix Lalpha(1, 6);
249
250 Lalpha[0][0] = 0;
251 Lalpha[0][1] = 0.;
252 Lalpha[0][2] = 0.;
253 Lalpha[0][3] = cos(alpha) * one_over_rho;
254 Lalpha[0][4] = sin(alpha) * one_over_rho;
255 Lalpha[0][5] = -1.;
256
257 L = vpMatrix::stack(L, Lalpha);
258 }
259
260 return L;
261}
262
275{
276 vpColVector e(0);
277
278 if (vpFeatureVanishingPoint::selectX() & select) {
279 vpColVector ex(1);
280 ex[0] = s[0] - s_star[0];
281
282 e = vpColVector::stack(e, ex);
283 }
284
285 if (vpFeatureVanishingPoint::selectY() & select) {
286 vpColVector ey(1);
287 ey[0] = s[1] - s_star[1];
288 e = vpColVector::stack(e, ey);
289 }
290
292 vpColVector e_one_over_rho(1);
293 e_one_over_rho[0] = s[2] - s_star[2];
294
295 e = vpColVector::stack(e, e_one_over_rho);
296 }
297
299 vpColVector e_atan_one_over_rho(1);
300 e_atan_one_over_rho[0] = s[3] - s_star[3];
301
302 e = vpColVector::stack(e, e_atan_one_over_rho);
303 }
304
306 vpColVector e_alpha(1);
307 double err = s[4] - s_star[4];
308
309 if (err < -M_PI)
310 err += 2 * M_PI;
311 if (err > M_PI)
312 err -= 2 * M_PI;
313
314 e_alpha[0] = err;
315 e = vpColVector::stack(e, e_alpha);
316 }
317
318 return e;
319}
320
329void vpFeatureVanishingPoint::print(unsigned int select) const
330{
331 std::cout << "Vanishing point:";
333 std::cout << " x=" << get_x();
335 std::cout << " y=" << get_y();
337 std::cout << " 1/rho=" << getOneOverRho();
338 }
340 std::cout << " atan(1/rho)=" << getAtanOneOverRho();
341 }
343 std::cout << " alpha=" << getAlpha();
344 }
345 std::cout << std::endl;
346}
347
348
351{
352 set_xy(x, y);
353}
354
364 const vpColor &color, unsigned int thickness) const
365{
367 double x, y;
368 x = get_x();
369 y = get_y();
370
371 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
372 }
374 double one_over_rho = getOneOverRho();
375 double alpha = getAlpha();
376 double x = cos(alpha) / one_over_rho;
377 double y = sin(alpha) / one_over_rho;
378 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
379 }
381 double atan_one_over_rho = getAtanOneOverRho();
382 double alpha = getAlpha();
383 double x = cos(alpha) / tan(atan_one_over_rho);
384 double y = sin(alpha) / tan(atan_one_over_rho);
385 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
386 }
387}
388
398 unsigned int thickness) const
399{
401 double x, y;
402 x = get_x();
403 y = get_y();
404
405 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
406 }
408 double one_over_rho = getOneOverRho();
409 double alpha = getAlpha();
410 double x = cos(alpha) / one_over_rho;
411 double y = sin(alpha) / one_over_rho;
412 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
413 }
415 double atan_one_over_rho = getAtanOneOverRho();
416 double alpha = getAlpha();
417 double x = cos(alpha) / tan(atan_one_over_rho);
418 double y = sin(alpha) / tan(atan_one_over_rho);
419 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
420 }
421}
422
427{
429 return feature;
430}
431
434
437
442
447
452
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
static unsigned int selectAtanOneOverRho()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=(selectX()|selectY()))
void set_y(double y)
Set vanishing point feature value.
void setAlpha(double alpha)
Set vanishing point feature value.
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpFeatureVanishingPoint * duplicate() const
vpMatrix interaction(unsigned int select=(selectX()|selectY()))
double getOneOverRho() const
Get vanishing point feature value.
double getAlpha() const
Get vanishing point feature value.
void print(unsigned int select=(selectX()|selectY())) const
void setAtanOneOverRho(double atan_one_over_rho)
Set vanishing point feature value.
void setOneOverRho(double one_over_rho)
Set vanishing point feature value.
double getAtanOneOverRho() const
Get vanishing point feature value.
void set_xy(double x, double y)
Set vanishing point visual feature from cartesian coordinates. Same as buildFrom().
static unsigned int selectX()
Select visual feature .
static unsigned int selectOneOverRho()
double get_y() const
Get vanishing point feature value.
static unsigned int selectAlpha()
void buildFrom(double x, double y)
Set vanishing point visual feature from cartesian coordinates. Same as set_xy().
double get_x() const
Get vanishing point feature value.
vpFeatureVanishingPoint()
Default constructor that calls init().
static unsigned int selectY()
Select visual feature .
void set_x(double x)
Set vanishing point feature value.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
#define vpTRACE
Definition: vpDebug.h:416