Visual Servoing Platform  version 3.6.1 under development (2024-10-02)
testMathUtils.cpp

Test additional math functions such as lon-lat generator or look-at function.

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test additional math functions such as lon-lat generator or look-at function.
*/
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_CATCH2
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#define CATCH_CONFIG_RUNNER
#include <catch.hpp>
// #define VERBOSE
// #define DEBUG
#ifdef DEBUG
#include <visp3/core/vpIoTools.h>
#endif
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
TEST_CASE("Lon-Lat generator", "[math_lonlat]")
{
const int lonStart = 0, lonEnd = 360, nlon = 20;
const int latStart = 0, latEnd = 90, nLat = 10;
std::vector<double> longitudes = vpMath::linspace(lonStart, lonEnd, nlon);
std::vector<double> latitudes = vpMath::linspace(latStart, latEnd, nLat);
const double radius = 5;
std::vector<std::pair<double, double> > lonlatVec;
lonlatVec.reserve(longitudes.size() * latitudes.size());
for (auto lon : longitudes) {
for (auto lat : latitudes) {
lonlatVec.emplace_back(lon, lat);
}
}
SECTION("NED")
{
std::vector<vpHomogeneousMatrix> ecef_M_ned_vec =
for (const auto &ecef_M_ned : ecef_M_ned_vec) {
#ifdef VERBOSE
std::cout << "Lon-Lat ecef_M_ned:\n" << ecef_M_ned << std::endl;
#endif
CHECK(ecef_M_ned.isValid());
CHECK(ecef_M_ned.getRotationMatrix().isARotationMatrix());
CHECK(vpMath::equal(ecef_M_ned.getTranslationVector().sumSquare(), radius * radius));
}
#ifdef DEBUG
ned_M_cv[0][0] = 0;
ned_M_cv[0][1] = -1;
ned_M_cv[1][0] = 1;
ned_M_cv[1][1] = 0;
const std::string folder = "NED/lon-lat/";
int i = 0;
for (const auto &ecef_M_ned : ecef_M_ned_vec) {
std::stringstream buffer;
buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
std::string filename = buffer.str();
std::ofstream file(filename);
if (file.is_open()) {
(ecef_M_ned * ned_M_cv).save(file);
}
}
#endif
}
SECTION("ENU")
{
std::vector<vpHomogeneousMatrix> ecef_M_enu_vec =
for (const auto &ecef_M_enu : ecef_M_enu_vec) {
#ifdef VERBOSE
std::cout << "Lon-Lat ecef_M_enu:\n" << ecef_M_enu << std::endl;
#endif
CHECK(ecef_M_enu.isValid());
CHECK(ecef_M_enu.getRotationMatrix().isARotationMatrix());
CHECK(vpMath::equal(ecef_M_enu.getTranslationVector().sumSquare(), radius * radius));
#ifdef DEBUG
enu_M_cv[1][1] = -1;
enu_M_cv[2][2] = -1;
const std::string folder = "ENU/lon-lat/";
int i = 0;
for (const auto &ecef_M_enu : ecef_M_enu_vec) {
std::stringstream buffer;
buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
std::string filename = buffer.str();
std::ofstream file(filename);
if (file.is_open()) {
(ecef_M_enu * enu_M_cv).save(file);
}
}
#endif
}
}
}
TEST_CASE("Equidistributed sphere point", "[math_equi_sphere_pts]")
{
const unsigned int maxPoints = 200;
std::vector<std::pair<double, double> > lonlatVec = vpMath::computeRegularPointsOnSphere(maxPoints);
const double radius = 5;
SECTION("NED")
{
std::vector<vpHomogeneousMatrix> ecef_M_ned_vec =
CHECK(!ecef_M_ned_vec.empty());
for (const auto &ecef_M_ned : ecef_M_ned_vec) {
#ifdef VERBOSE
std::cout << "Equidistributed ecef_M_ned:\n" << ecef_M_ned << std::endl;
#endif
CHECK(ecef_M_ned.isValid());
CHECK(ecef_M_ned.getRotationMatrix().isARotationMatrix());
CHECK(vpMath::equal(ecef_M_ned.getTranslationVector().sumSquare(), radius * radius));
}
#ifdef DEBUG
ned_M_cv[0][0] = 0;
ned_M_cv[0][1] = -1;
ned_M_cv[1][0] = 1;
ned_M_cv[1][1] = 0;
const std::string folder = "NED/equi/";
int i = 0;
for (const auto &ecef_M_ned : ecef_M_ned_vec) {
std::stringstream buffer;
buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
std::string filename = buffer.str();
std::ofstream file(filename);
if (file.is_open()) {
(ecef_M_ned * ned_M_cv).save(file);
}
}
#endif
}
SECTION("ENU")
{
std::vector<vpHomogeneousMatrix> ecef_M_enu_vec =
CHECK(!ecef_M_enu_vec.empty());
for (const auto &ecef_M_enu : ecef_M_enu_vec) {
#ifdef VERBOSE
std::cout << "Equidistributed ecef_M_enu:\n" << ecef_M_enu << std::endl;
#endif
CHECK(ecef_M_enu.isValid());
CHECK(ecef_M_enu.getRotationMatrix().isARotationMatrix());
CHECK(vpMath::equal(ecef_M_enu.getTranslationVector().sumSquare(), radius * radius));
}
#ifdef DEBUG
enu_M_cv[1][1] = -1;
enu_M_cv[2][2] = -1;
const std::string folder = "ENU/equi/";
int i = 0;
for (const auto &ecef_M_enu : ecef_M_enu_vec) {
std::stringstream buffer;
buffer << folder << "ecef_M_cv_" << std::setw(4) << std::setfill('0') << i++ << ".txt";
std::string filename = buffer.str();
std::ofstream file(filename);
if (file.is_open()) {
(ecef_M_enu * enu_M_cv).save(file);
}
}
#endif
}
}
TEST_CASE("Look-at", "[math_look_at]")
{
// Set camera to an arbitrary pose (only translation)
vpColVector from_blender = { 8.867762565612793, -1.1965436935424805, 2.1211400032043457 };
// Transformation from OpenGL to Blender frame
vpHomogeneousMatrix blender_M_gl;
blender_M_gl[0][0] = 0;
blender_M_gl[0][2] = 1;
blender_M_gl[1][0] = 1;
blender_M_gl[1][1] = 0;
blender_M_gl[2][1] = 1;
blender_M_gl[2][2] = 0;
// From is the current camera pose expressed in the OpenGL coordinate system
vpColVector from = (blender_M_gl.getRotationMatrix().t() * from_blender);
// To is the desired point toward the camera must look
vpColVector to = { 0, 0, 0 };
// Up is an arbitrary vector
vpColVector up = { 0, 1, 0 };
// Compute the look-at transformation
vpHomogeneousMatrix gl_M_cam = vpMath::lookAt(from, to, up);
std::cout << "\ngl_M_cam:\n" << gl_M_cam << std::endl;
// Transformation from the computer vision frame to the Blender camera frame
cam_M_cv[1][1] = -1;
cam_M_cv[2][2] = -1;
// Transformation from the computer vision frame to the Blender frame
vpHomogeneousMatrix bl_M_cv = blender_M_gl * gl_M_cam * cam_M_cv;
std::cout << "\nbl_M_cv:\n" << bl_M_cv << std::endl;
// Ground truth using Blender look-at
vpHomogeneousMatrix bl_M_cv_gt = vpHomogeneousMatrix({ 0.13372008502483368, 0.22858507931232452, -0.9642965197563171,
8.867762565612793, 0.9910191297531128, -0.030843468382954597,
0.13011434674263, -1.1965436935424805, -5.4016709327697754e-08,
-0.9730352163314819, -0.23065657913684845, 2.121140241622925 });
std::cout << "\nbl_M_cv_gt:\n" << bl_M_cv_gt << std::endl;
const double tolerance = 1e-6;
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = 0; j < 4; j++) {
CHECK(vpMath::equal(bl_M_cv[i][j], bl_M_cv_gt[i][j], tolerance));
}
}
}
int main(int argc, char *argv[])
{
Catch::Session session; // There must be exactly one instance
// Let Catch (using Clara) parse the command line
session.applyCommandLine(argc, argv);
int numFailed = session.run();
// numFailed is clamped to 255 as some unices only use the lower 8 bits.
// This clamping has already been applied, so just return it here
// You can also do any post run clean-up here
return numFailed;
}
#else
#include <iostream>
int main() { return EXIT_SUCCESS; }
#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
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:550
static std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, LongLattToHomogeneous func)
Definition: vpMath.cpp:652
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition: vpMath.cpp:688
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:561
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
Definition: vpMath.cpp:601
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Definition: vpMath.h:333
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:497
vpRotationMatrix t() const