Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
58TEST_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/";
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
141TEST_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/";
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/";
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
216TEST_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
263int 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
280int main() { return EXIT_SUCCESS; }
281#endif
Implementation of column vector and the associated operations.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
static void makeDirectory(const std::string &dirname)
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:622
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Definition vpMath.h:250
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition vpMath.cpp:657
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
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 vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition vpMath.cpp:475