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>
#ifdef DEBUG
#include <visp3/core/vpIoTools.h>
#endif
#ifdef ENABLE_VISP_NAMESPACE
#endif
TEST_CASE("Lon-Lat generator", "[math_lonlat]")
{
const int lonStart = 0, lonEnd = 360, nlon = 20;
const int latStart = 0, latEnd = 90, nLat = 10;
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;
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]")
{
vpColVector from_blender = { 8.867762565612793, -1.1965436935424805, 2.1211400032043457 };
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;
std::cout << "\ngl_M_cam:\n" << gl_M_cam << std::endl;
cam_M_cv[1][1] = -1;
cam_M_cv[2][2] = -1;
std::cout << "\nbl_M_cv:\n" << bl_M_cv << std::endl;
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;
session.applyCommandLine(argc, argv);
int numFailed = session.run();
return numFailed;
}
#else
#include <iostream>
int main() { return EXIT_SUCCESS; }
#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 std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, LongLattToHomogeneous func)
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
static bool equal(double x, double y, double threshold=0.001)
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
vpRotationMatrix t() const