Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureVanishingPoint.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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
349void vpFeatureVanishingPoint::buildFrom(double x, double y) { set_xy(x, y); }
350
360 const vpColor &color, unsigned int thickness) const
361{
363 double x, y;
364 x = get_x();
365 y = get_y();
366
367 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
369 double one_over_rho = getOneOverRho();
370 double alpha = getAlpha();
371 double x = cos(alpha) / one_over_rho;
372 double y = sin(alpha) / one_over_rho;
373 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
375 double atan_one_over_rho = getAtanOneOverRho();
376 double alpha = getAlpha();
377 double x = cos(alpha) / tan(atan_one_over_rho);
378 double y = sin(alpha) / tan(atan_one_over_rho);
379 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
380 }
381}
382
392 unsigned int thickness) const
393{
395 double x, y;
396 x = get_x();
397 y = get_y();
398
399 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
401 double one_over_rho = getOneOverRho();
402 double alpha = getAlpha();
403 double x = cos(alpha) / one_over_rho;
404 double y = sin(alpha) / one_over_rho;
405 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
407 double atan_one_over_rho = getAtanOneOverRho();
408 double alpha = getAlpha();
409 double x = cos(alpha) / tan(atan_one_over_rho);
410 double y = sin(alpha) / tan(atan_one_over_rho);
411 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
412 }
413}
414
423
426
429
434
439
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.
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
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.
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.
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void stack(const vpMatrix &A)
#define vpTRACE
Definition vpDebug.h:411