Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testMathUtils.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  * Test additional math functions such as lon-lat generator or look-at function.
33  *
34 *****************************************************************************/
35 
41 #include <visp3/core/vpConfig.h>
42 
43 #ifdef VISP_HAVE_CATCH2
44 
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpMath.h>
47 
48 #define CATCH_CONFIG_RUNNER
49 #include <catch.hpp>
50 
51 // #define VERBOSE
52 // #define DEBUG
53 
54 #ifdef DEBUG
55 #include <visp3/core/vpIoTools.h>
56 #endif
57 
58 TEST_CASE("Lon-Lat generator", "[math_lonlat]")
59 {
60  const int lonStart = 0, lonEnd = 360, nlon = 20;
61  const int latStart = 0, latEnd = 90, nLat = 10;
62  std::vector<double> longitudes = vpMath::linspace(lonStart, lonEnd, nlon);
63  std::vector<double> latitudes = vpMath::linspace(latStart, latEnd, nLat);
64  const double radius = 5;
65 
66  std::vector<std::pair<double, double> > lonlatVec;
67  lonlatVec.reserve(longitudes.size() * latitudes.size());
68  for (auto lon : longitudes) {
69  for (auto lat : latitudes) {
70  lonlatVec.emplace_back(lon, lat);
71  }
72  }
73 
74  SECTION("NED")
75  {
76  std::vector<vpHomogeneousMatrix> ecef_M_ned_vec =
78  for (const auto &ecef_M_ned : ecef_M_ned_vec) {
79 #ifdef VERBOSE
80  std::cout << "Lon-Lat ecef_M_ned:\n" << ecef_M_ned << std::endl;
81 #endif
82  CHECK(ecef_M_ned.isValid());
83  CHECK(ecef_M_ned.getRotationMatrix().isARotationMatrix());
84  CHECK(vpMath::equal(ecef_M_ned.getTranslationVector().sumSquare(), radius * radius));
85  }
86 
87 #ifdef DEBUG
88  vpHomogeneousMatrix ned_M_cv;
89  ned_M_cv[0][0] = 0;
90  ned_M_cv[0][1] = -1;
91  ned_M_cv[1][0] = 1;
92  ned_M_cv[1][1] = 0;
93  const std::string folder = "NED/lon-lat/";
95  int i = 0;
96  for (const auto &ecef_M_ned : ecef_M_ned_vec) {
97  std::stringstream buffer;
98  buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
99  std::string filename = buffer.str();
100  std::ofstream file(filename);
101  if (file.is_open()) {
102  (ecef_M_ned * ned_M_cv).save(file);
103  }
104  }
105 #endif
106  }
107 
108  SECTION("ENU")
109  {
110  std::vector<vpHomogeneousMatrix> ecef_M_enu_vec =
112  for (const auto &ecef_M_enu : ecef_M_enu_vec) {
113 #ifdef VERBOSE
114  std::cout << "Lon-Lat ecef_M_enu:\n" << ecef_M_enu << std::endl;
115 #endif
116  CHECK(ecef_M_enu.isValid());
117  CHECK(ecef_M_enu.getRotationMatrix().isARotationMatrix());
118  CHECK(vpMath::equal(ecef_M_enu.getTranslationVector().sumSquare(), radius * radius));
119 
120 #ifdef DEBUG
121  vpHomogeneousMatrix enu_M_cv;
122  enu_M_cv[1][1] = -1;
123  enu_M_cv[2][2] = -1;
124  const std::string folder = "ENU/lon-lat/";
125  vpIoTools::makeDirectory(folder);
126  int i = 0;
127  for (const auto &ecef_M_enu : ecef_M_enu_vec) {
128  std::stringstream buffer;
129  buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
130  std::string filename = buffer.str();
131  std::ofstream file(filename);
132  if (file.is_open()) {
133  (ecef_M_enu * enu_M_cv).save(file);
134  }
135  }
136 #endif
137  }
138  }
139 }
140 
141 TEST_CASE("Equidistributed sphere point", "[math_equi_sphere_pts]")
142 {
143  const unsigned int maxPoints = 200;
144  std::vector<std::pair<double, double> > lonlatVec = vpMath::computeRegularPointsOnSphere(maxPoints);
145  const double radius = 5;
146 
147  SECTION("NED")
148  {
149  std::vector<vpHomogeneousMatrix> ecef_M_ned_vec =
151  CHECK(!ecef_M_ned_vec.empty());
152  for (const auto &ecef_M_ned : ecef_M_ned_vec) {
153 #ifdef VERBOSE
154  std::cout << "Equidistributed ecef_M_ned:\n" << ecef_M_ned << std::endl;
155 #endif
156  CHECK(ecef_M_ned.isValid());
157  CHECK(ecef_M_ned.getRotationMatrix().isARotationMatrix());
158  CHECK(vpMath::equal(ecef_M_ned.getTranslationVector().sumSquare(), radius * radius));
159  }
160 
161 #ifdef DEBUG
162  vpHomogeneousMatrix ned_M_cv;
163  ned_M_cv[0][0] = 0;
164  ned_M_cv[0][1] = -1;
165  ned_M_cv[1][0] = 1;
166  ned_M_cv[1][1] = 0;
167  const std::string folder = "NED/equi/";
168  vpIoTools::makeDirectory(folder);
169  int i = 0;
170  for (const auto &ecef_M_ned : ecef_M_ned_vec) {
171  std::stringstream buffer;
172  buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
173  std::string filename = buffer.str();
174  std::ofstream file(filename);
175  if (file.is_open()) {
176  (ecef_M_ned * ned_M_cv).save(file);
177  }
178  }
179 #endif
180  }
181 
182  SECTION("ENU")
183  {
184  std::vector<vpHomogeneousMatrix> ecef_M_enu_vec =
186  CHECK(!ecef_M_enu_vec.empty());
187  for (const auto &ecef_M_enu : ecef_M_enu_vec) {
188 #ifdef VERBOSE
189  std::cout << "Equidistributed ecef_M_enu:\n" << ecef_M_enu << std::endl;
190 #endif
191  CHECK(ecef_M_enu.isValid());
192  CHECK(ecef_M_enu.getRotationMatrix().isARotationMatrix());
193  CHECK(vpMath::equal(ecef_M_enu.getTranslationVector().sumSquare(), radius * radius));
194  }
195 
196 #ifdef DEBUG
197  vpHomogeneousMatrix enu_M_cv;
198  enu_M_cv[1][1] = -1;
199  enu_M_cv[2][2] = -1;
200  const std::string folder = "ENU/equi/";
201  vpIoTools::makeDirectory(folder);
202  int i = 0;
203  for (const auto &ecef_M_enu : ecef_M_enu_vec) {
204  std::stringstream buffer;
205  buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
206  std::string filename = buffer.str();
207  std::ofstream file(filename);
208  if (file.is_open()) {
209  (ecef_M_enu * enu_M_cv).save(file);
210  }
211  }
212 #endif
213  }
214 }
215 
216 TEST_CASE("Look-at", "[math_look_at]")
217 {
218  // Set camera to an arbitrary pose (only translation)
219  vpColVector from_blender = {8.867762565612793, -1.1965436935424805, 2.1211400032043457};
220  // Transformation from OpenGL to Blender frame
221  vpHomogeneousMatrix blender_M_gl;
222  blender_M_gl[0][0] = 0;
223  blender_M_gl[0][2] = 1;
224  blender_M_gl[1][0] = 1;
225  blender_M_gl[1][1] = 0;
226  blender_M_gl[2][1] = 1;
227  blender_M_gl[2][2] = 0;
228 
229  // From is the current camera pose expressed in the OpenGL coordinate system
230  vpColVector from = (blender_M_gl.getRotationMatrix().t() * from_blender);
231  // To is the desired point toward the camera must look
232  vpColVector to = {0, 0, 0};
233  // Up is an arbitrary vector
234  vpColVector up = {0, 1, 0};
235 
236  // Compute the look-at transformation
237  vpHomogeneousMatrix gl_M_cam = vpMath::lookAt(from, to, up);
238  std::cout << "\ngl_M_cam:\n" << gl_M_cam << std::endl;
239 
240  // Transformation from the computer vision frame to the Blender camera frame
241  vpHomogeneousMatrix cam_M_cv;
242  cam_M_cv[1][1] = -1;
243  cam_M_cv[2][2] = -1;
244  // Transformation from the computer vision frame to the Blender frame
245  vpHomogeneousMatrix bl_M_cv = blender_M_gl * gl_M_cam * cam_M_cv;
246  std::cout << "\nbl_M_cv:\n" << bl_M_cv << std::endl;
247 
248  // Ground truth using Blender look-at
249  vpHomogeneousMatrix bl_M_cv_gt = {0.13372008502483368, 0.22858507931232452, -0.9642965197563171,
250  8.867762565612793, 0.9910191297531128, -0.030843468382954597,
251  0.13011434674263, -1.1965436935424805, -5.4016709327697754e-08,
252  -0.9730352163314819, -0.23065657913684845, 2.121140241622925};
253  std::cout << "\nbl_M_cv_gt:\n" << bl_M_cv_gt << std::endl;
254 
255  const double tolerance = 1e-6;
256  for (unsigned int i = 0; i < 3; i++) {
257  for (unsigned int j = 0; j < 4; j++) {
258  CHECK(vpMath::equal(bl_M_cv[i][j], bl_M_cv_gt[i][j], tolerance));
259  }
260  }
261 }
262 
263 int main(int argc, char *argv[])
264 {
265  Catch::Session session; // There must be exactly one instance
266 
267  // Let Catch (using Clara) parse the command line
268  session.applyCommandLine(argc, argv);
269 
270  int numFailed = session.run();
271 
272  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
273  // This clamping has already been applied, so just return it here
274  // You can also do any post run clean-up here
275  return numFailed;
276 }
277 #else
278 #include <iostream>
279 
280 int main() { return EXIT_SUCCESS; }
281 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:972
static std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius))
Definition: vpMath.cpp:623
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition: vpMath.cpp:658
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:536
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
Definition: vpMath.cpp:572
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Definition: vpMath.h:330
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:475
vpRotationMatrix t() const