Visual Servoing Platform version 3.5.0
ViewController.mm
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 *****************************************************************************/
32
33#ifndef DOXYGEN_SHOULD_SKIP_THIS
34
35#import "ViewController.h"
36#ifdef __cplusplus
37#import <visp3/visp.h>
38#endif
39
40@interface ViewController ()
41@end
42
43@implementation ViewController
44#pragma mark - Example of a function that uses ViSP
45- (void)processViSPHomography{
46
47 std::vector<vpPoint> oP(4), aP(4), bP(4);
48 double L = 0.1;
49
50 oP[0].setWorldCoordinates( -L,-L, 0);
51 oP[1].setWorldCoordinates(2*L,-L, 0);
52 oP[2].setWorldCoordinates( L, 3*L, 0);
53 oP[3].setWorldCoordinates( -L, 4*L, 0);
54
55 vpHomogeneousMatrix bMo(0,0, 1, 0, 0, 0) ;
56 vpHomogeneousMatrix aMb(0.2, 0, 0.1, 0,vpMath::rad(20), 0);
57 vpHomogeneousMatrix aMo = aMb*bMo ;
58
59 // Normalized coordinates of points in the image frame
60 std::vector<double> xa(4), ya(4), xb(4), yb(4);
61
62 for(int i=0 ; i < 4; i++){
63 oP[i].project(aMo);
64 xa[i] = oP[i].get_x();
65 ya[i] = oP[i].get_y();
66 oP[i].project(bMo);
67 xb[i] = oP[i].get_x();
68 yb[i] = oP[i].get_y();
69 }
70
71 vpHomography aHb ;
72
73 // Compute the homography
74 vpHomography::DLT(xb, yb, xa, ya, aHb, true);
75
76 std::cout << "Homography:\n" << aHb << std::endl;
77
81
82 // Compute the 3D transformation
83 aHb.computeDisplacement(aRb, atb, n);
84
85 std::cout << "atb: " << atb.t() << std::endl;
86
87 // Compute coordinates in pixels of point 3
88 vpImagePoint iPa, iPb;
90 vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb);
91 vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa);
92
93 std::cout << "Ground truth:" << std::endl;
94 std::cout << " Point 3 in pixels in frame b: " << iPb << std::endl;
95 std::cout << " Point 3 in pixels in frame a: " << iPa << std::endl;
96
97 // Estimate the position in pixel of point 3 from the homography
98 vpMatrix H = cam.get_K() * aHb * cam.get_K_inverse();
99
100 // Project the position in pixel of point 3 from the homography
101 std::cout << "Estimation from homography:" << std::endl;
102 std::cout << " Point 3 in pixels in frame a: " << vpHomography::project(cam, aHb, iPb) << std::endl;
103}
104- (void)viewDidLoad {
105 [super viewDidLoad];
106 // Do any additional setup after loading the view, typically from a nib.
107 [self processViSPHomography];
108}
109- (void)didReceiveMemoryWarning {
110 [super didReceiveMemoryWarning];
111 // Dispose of any resources that can be recreated.
112}
113@end
114
115#endif
116
Generic class defining intrinsic camera parameters.
vpMatrix get_K_inverse() const
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
static void DLT(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, bool normalization=true)
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.
vpRowVector t() const