diff --git a/3rdparty/simdlib/Simd/SimdBaseCustomFunctions.cpp b/3rdparty/simdlib/Simd/SimdBaseCustomFunctions.cpp index 45f68939c1..72f3d49cd0 100644 --- a/3rdparty/simdlib/Simd/SimdBaseCustomFunctions.cpp +++ b/3rdparty/simdlib/Simd/SimdBaseCustomFunctions.cpp @@ -22,245 +22,249 @@ namespace Simd { - namespace Base - { - void ImageErosion(uint8_t * img, const uint8_t * buff, size_t width, size_t height, SimdImageConnexityType connexityType) - { - const size_t buffWidth = width + 2; - if (connexityType == SimdImageConnexity4) { - size_t offset[5] = {1, buffWidth, buffWidth + 1, buffWidth + 2, buffWidth * 2 + 1}; - - for (size_t i = 0; i < height; i++) { - const uint8_t *ptr_buff = buff + i * buffWidth; - uint8_t *ptr_img = img + i * width; +namespace Base +{ +void ImageErosion(uint8_t *img, const uint8_t *buff, size_t width, size_t height, SimdImageConnexityType connexityType) +{ + const size_t buffWidth = width + 2; + if (connexityType == SimdImageConnexity4) { + size_t offset[5] = { 1, buffWidth, buffWidth + 1, buffWidth + 2, buffWidth * 2 + 1 }; - for (size_t j = 0; j < width; j++) { - uint8_t min_value = 255; - for (int k = 0; k < 5; k++) { - min_value = (std::min)(min_value, *(ptr_buff + j + offset[k])); - } + for (size_t i = 0; i < height; i++) { + const uint8_t *ptr_buff = buff + i * buffWidth; + uint8_t *ptr_img = img + i * width; - *(ptr_img + j) = min_value; - } - } - } else { - size_t offset[9] = { 0, - 1, - 2, - buffWidth, - buffWidth + 1, - buffWidth + 2, - buffWidth * 2, - buffWidth * 2 + 1, - buffWidth * 2 + 2 }; + for (size_t j = 0; j < width; j++) { + uint8_t min_value = 255; + for (int k = 0; k < 5; k++) { + min_value = (std::min)(min_value, *(ptr_buff + j + offset[k])); + } - for (size_t i = 0; i < height; i++) { - const uint8_t *ptr_buff = buff + i * buffWidth; - uint8_t *ptr_img = img + i * width; + *(ptr_img + j) = min_value; + } + } + } + else { + size_t offset[9] = { 0, + 1, + 2, + buffWidth, + buffWidth + 1, + buffWidth + 2, + buffWidth * 2, + buffWidth * 2 + 1, + buffWidth * 2 + 2 }; - for (size_t j = 0; j < width; j++) { - uint8_t min_value = 255; - for (int k = 0; k < 9; k++) { - min_value = (std::min)(min_value, *(ptr_buff + j + offset[k])); - } + for (size_t i = 0; i < height; i++) { + const uint8_t *ptr_buff = buff + i * buffWidth; + uint8_t *ptr_img = img + i * width; - *(ptr_img + j) = min_value; - } - } - } + for (size_t j = 0; j < width; j++) { + uint8_t min_value = 255; + for (int k = 0; k < 9; k++) { + min_value = (std::min)(min_value, *(ptr_buff + j + offset[k])); } - void ImageDilatation(uint8_t * img, const uint8_t * buff, size_t width, size_t height, SimdImageConnexityType connexityType) - { - const size_t buffWidth = width + 2; - if (connexityType == SimdImageConnexity4) { - size_t offset[5] = {1, buffWidth, buffWidth + 1, buffWidth + 2, buffWidth * 2 + 1}; + *(ptr_img + j) = min_value; + } + } + } +} - for (size_t i = 0; i < height; i++) { - const uint8_t *ptr_buff = buff + i * buffWidth; - uint8_t *ptr_img = img + i * width; +void ImageDilatation(uint8_t *img, const uint8_t *buff, size_t width, size_t height, SimdImageConnexityType connexityType) +{ + const size_t buffWidth = width + 2; + if (connexityType == SimdImageConnexity4) { + size_t offset[5] = { 1, buffWidth, buffWidth + 1, buffWidth + 2, buffWidth * 2 + 1 }; - for (size_t j = 0; j < width; j++) { - uint8_t max_value = 0; - for (int k = 0; k < 5; k++) { - max_value = (std::max)(max_value, *(ptr_buff + j + offset[k])); - } + for (size_t i = 0; i < height; i++) { + const uint8_t *ptr_buff = buff + i * buffWidth; + uint8_t *ptr_img = img + i * width; - *(ptr_img + j) = max_value; - } - } - } else { - size_t offset[9] = { 0, - 1, - 2, - buffWidth, - buffWidth + 1, - buffWidth + 2, - buffWidth * 2, - buffWidth * 2 + 1, - buffWidth * 2 + 2 }; + for (size_t j = 0; j < width; j++) { + uint8_t max_value = 0; + for (int k = 0; k < 5; k++) { + max_value = (std::max)(max_value, *(ptr_buff + j + offset[k])); + } - for (size_t i = 0; i < height; i++) { - const uint8_t *ptr_buff = buff + i * buffWidth; - uint8_t *ptr_img = img + i * width; + *(ptr_img + j) = max_value; + } + } + } + else { + size_t offset[9] = { 0, + 1, + 2, + buffWidth, + buffWidth + 1, + buffWidth + 2, + buffWidth * 2, + buffWidth * 2 + 1, + buffWidth * 2 + 2 }; - for (size_t j = 0; j < width; j++) { - uint8_t max_value = 0; - for (int k = 0; k < 9; k++) { - max_value = (std::max)(max_value, *(ptr_buff + j + offset[k])); - } + for (size_t i = 0; i < height; i++) { + const uint8_t *ptr_buff = buff + i * buffWidth; + uint8_t *ptr_img = img + i * width; - *(ptr_img + j) = max_value; - } - } - } + for (size_t j = 0; j < width; j++) { + uint8_t max_value = 0; + for (int k = 0; k < 9; k++) { + max_value = (std::max)(max_value, *(ptr_buff + j + offset[k])); } - double SimdVectorSum(const double * vec, size_t size) - { - double sum = 0.0; - for (size_t i = 0; i < size; i++) { - sum += vec[i]; - } - return sum; - } + *(ptr_img + j) = max_value; + } + } + } +} - double SimdVectorSumSquare(const double * vec, size_t size) - { - double sum_square = 0.0; - for (size_t i = 0; i < size; i++) { - sum_square += vec[i] * vec[i]; - } - return sum_square; - } +double SimdVectorSum(const double *vec, size_t size) +{ + double sum = 0.0; + for (size_t i = 0; i < size; i++) { + sum += vec[i]; + } + return sum; +} - double SimdVectorStdev(const double * vec, size_t size, bool useBesselCorrection) - { - double mean_value = SimdVectorSum(vec, size) / size; - double sum_squared_diff = 0.0; - for (size_t i = 0; i < size; i++) { - sum_squared_diff += (vec[i] - mean_value) * (vec[i] - mean_value); - } +double SimdVectorSumSquare(const double *vec, size_t size) +{ + double sum_square = 0.0; + for (size_t i = 0; i < size; i++) { + sum_square += vec[i] * vec[i]; + } + return sum_square; +} - double divisor = (double)size; - if (useBesselCorrection) { - divisor = divisor - 1; - } +double SimdVectorStdev(const double *vec, size_t size, bool useBesselCorrection) +{ + double mean_value = SimdVectorSum(vec, size) / size; + double sum_squared_diff = 0.0; + for (size_t i = 0; i < size; i++) { + sum_squared_diff += (vec[i] - mean_value) * (vec[i] - mean_value); + } - return std::sqrt(sum_squared_diff / divisor); - } + double divisor = (double)size; + if (useBesselCorrection) { + divisor = divisor - 1; + } - void SimdVectorHadamard(const double * src1, const double * src2, size_t size, double * dst) - { - for (size_t i = 0; i < size; i++) { - dst[i] = src1[i] * src2[i]; - } - } + return std::sqrt(sum_squared_diff / divisor); +} - void SimdMatMulTwist(const double * mat, size_t rows, const double * twist, double * dst) - { - for (size_t i = 0; i < rows; i++) { - for (size_t j = 0; j < 6; j++) { - double s = 0; - for (size_t k = 0; k < 6; k++) { - s += mat[i*6 + k] * twist[k*6 + j]; - } - dst[i*6 + j] = s; - } - } - } +void SimdVectorHadamard(const double *src1, const double *src2, size_t size, double *dst) +{ + for (size_t i = 0; i < size; i++) { + dst[i] = src1[i] * src2[i]; + } +} - void SimdMatTranspose(const double * mat, size_t rows, size_t cols, double * dst) - { - if (rows <= 16 || cols <= 16) { - for (size_t i = 0; i < rows; i++) { - for (size_t j = 0; j < cols; j++) { - dst[j*cols + i] = mat[i*cols + j]; - } - } - } else { - // https://stackoverflow.com/a/21548079 - const size_t tileSize = 32; - for (size_t i = 0; i < rows; i += tileSize) { - for (size_t j = 0; j < cols; j++) { - for (size_t b = 0; b < tileSize && i + b < rows; b++) { - dst[i*cols + i+b] = mat[(i+b)*cols + j]; - } - } - } - } - } +void SimdMatMulTwist(const double *mat, size_t rows, const double *twist, double *dst) +{ + for (size_t i = 0; i < rows; i++) { + for (size_t j = 0; j < 6; j++) { + double s = 0; + for (size_t k = 0; k < 6; k++) { + s += mat[i*6 + k] * twist[k*6 + j]; + } + dst[i*6 + j] = s; + } + } +} - void SimdImageDifference(const unsigned char * img1, const unsigned char * img2, size_t size, unsigned char * imgDiff) - { - for (size_t i = 0; i < size; i++) { - int diff = img1[i] - img2[i] + 128; - imgDiff[i] = static_cast(std::max(std::min(diff, 255), 0)); - } +void SimdMatTranspose(const double *mat, size_t rows, size_t cols, double *dst) +{ + if (rows <= 16 || cols <= 16) { + for (size_t i = 0; i < rows; i++) { + for (size_t j = 0; j < cols; j++) { + dst[j*cols + i] = mat[i*cols + j]; + } + } + } + else { + // https://stackoverflow.com/a/21548079 + const size_t tileSize = 32; + for (size_t i = 0; i < rows; i += tileSize) { + for (size_t j = 0; j < cols; j++) { + for (size_t b = 0; b < tileSize && i + b < rows; b++) { + dst[j*rows + i+b] = mat[(i+b)*cols + j]; } + } + } + } +} - void SimdNormalizedCorrelation(const double * img1, double mean1, const double * img2, double mean2, size_t size, - double& a2, double& b2, double& ab) - { - for (size_t cpt = 0; cpt < size; cpt++) { - ab += (img1[cpt] - mean1) * (img2[cpt] - mean2); - a2 += (img1[cpt] - mean1) * (img1[cpt] - mean1); - b2 += (img2[cpt] - mean2) * (img2[cpt] - mean2); - } - } +void SimdImageDifference(const unsigned char *img1, const unsigned char *img2, size_t size, unsigned char *imgDiff) +{ + for (size_t i = 0; i < size; i++) { + int diff = img1[i] - img2[i] + 128; + imgDiff[i] = static_cast(std::max(std::min(diff, 255), 0)); + } +} - void SimdNormalizedCorrelation2(const double * img1, size_t width1, const double * img2, - size_t width2, size_t height2, size_t i0, size_t j0, double& ab) - { - for (size_t i = 0; i < height2; i++) { - for (size_t j = 0; j < width2; j++) { - ab += img1[(i0 + i)*width1 + j0 + j] * img2[i*width2 + j]; - } - } - } +void SimdNormalizedCorrelation(const double *img1, double mean1, const double *img2, double mean2, size_t size, + double &a2, double &b2, double &ab) +{ + for (size_t cpt = 0; cpt < size; cpt++) { + ab += (img1[cpt] - mean1) * (img2[cpt] - mean2); + a2 += (img1[cpt] - mean1) * (img1[cpt] - mean1); + b2 += (img2[cpt] - mean2) * (img2[cpt] - mean2); + } +} - static float lerp(float A, float B, float t) - { - return A * (1.0f - t) + B * t; - } +void SimdNormalizedCorrelation2(const double *img1, size_t width1, const double *img2, + size_t width2, size_t height2, size_t i0, size_t j0, double &ab) +{ + for (size_t i = 0; i < height2; i++) { + for (size_t j = 0; j < width2; j++) { + ab += img1[(i0 + i)*width1 + j0 + j] * img2[i*width2 + j]; + } + } +} - void SimdRemap(const unsigned char * src, size_t channels, size_t width, size_t height, size_t offset, - const int * mapU, const int * mapV, const float * mapDu, const float * mapDv, unsigned char * dst) - { - for (size_t j = 0; j < width; j++) { - int u_round = mapU[offset + j]; - int v_round = mapV[offset + j]; +static float lerp(float A, float B, float t) +{ + return A * (1.0f - t) + B * t; +} - float du = mapDu[offset + j]; - float dv = mapDv[offset + j]; +void SimdRemap(const unsigned char *src, size_t channels, size_t width, size_t height, size_t offset, + const int *mapU, const int *mapV, const float *mapDu, const float *mapDv, unsigned char *dst) +{ + for (size_t j = 0; j < width; j++) { + int u_round = mapU[offset + j]; + int v_round = mapV[offset + j]; - if (0 <= u_round && 0 <= v_round && u_round < static_cast(width) - 1 - && v_round < static_cast(height) - 1) { - for (size_t c = 0; c < channels; c++) { - // process interpolation - float col0 = lerp(src[(v_round*width + u_round)*channels + c], src[(v_round*width + u_round + 1)*channels + c], du); - float col1 = lerp(src[((v_round + 1)*width + u_round)*channels + c], src[((v_round + 1)*width + u_round + 1)*channels + c], du); - float value = lerp(col0, col1, dv); + float du = mapDu[offset + j]; + float dv = mapDv[offset + j]; - dst[(offset + j)*channels + c] = static_cast(value); - } - } else { - for (size_t c = 0; c < channels; c++) { - dst[(offset + j)*channels + c] = 0; - } - } - } - } + if (0 <= u_round && 0 <= v_round && u_round < static_cast(width) - 1 + && v_round < static_cast(height) - 1) { + for (size_t c = 0; c < channels; c++) { + // process interpolation + float col0 = lerp(src[(v_round*width + u_round)*channels + c], src[(v_round*width + u_round + 1)*channels + c], du); + float col1 = lerp(src[((v_round + 1)*width + u_round)*channels + c], src[((v_round + 1)*width + u_round + 1)*channels + c], du); + float value = lerp(col0, col1, dv); - void SimdComputeJtR(const double * J, size_t rows, const double * R, double * dst) - { - for (size_t i = 0; i < 6; i++) { - double ssum = 0; - for (size_t j = 0; j < rows; j++) { - ssum += J[j*6 + i] * R[j]; - } - dst[i] = ssum; - } - } + dst[(offset + j)*channels + c] = static_cast(value); + } + } + else { + for (size_t c = 0; c < channels; c++) { + dst[(offset + j)*channels + c] = 0; + } } + } +} + +void SimdComputeJtR(const double *J, size_t rows, const double *R, double *dst) +{ + for (size_t i = 0; i < 6; i++) { + double ssum = 0; + for (size_t j = 0; j < rows; j++) { + ssum += J[j*6 + i] * R[j]; + } + dst[i] = ssum; + } +} +} } diff --git a/doc/biblio/references.bib b/doc/biblio/references.bib index 13c3f81ca0..9c38a5ecd7 100644 --- a/doc/biblio/references.bib +++ b/doc/biblio/references.bib @@ -674,6 +674,7 @@ @article{Malvar2004HighqualityLI volume = {3}, pages = {iii-485} } + @inproceedings{Labbe2022Megapose, title = {{{MegaPose}}: {{6D Pose Estimation}} of {{Novel Objects}} via {{Render}} \& {{Compare}}}, booktitle = {CoRL}, @@ -681,6 +682,29 @@ @inproceedings{Labbe2022Megapose date = {2022} } +@ARTICLE{Marchand19a, + author={Marchand, Eric}, + journal={IEEE Robotics and Automation Letters}, + title={Subspace-Based Direct Visual Servoing}, + year={2019}, + volume={4}, + number={3}, + pages={2699-2706}, + keywords={Visual servoing;Feature extraction;Principal component analysis;Voltage control;Image reconstruction;Task analysis;Cameras;Visual servoing;sensor-based control}, + doi={10.1109/LRA.2019.2916263} +} + +@ARTICLE{Marchand20a, + author={Marchand, Eric}, + journal={IEEE Robotics and Automation Letters}, + title={Direct Visual Servoing in the Frequency Domain}, + year={2020}, + volume={5}, + number={2}, + pages={620-627}, + keywords={Discrete cosine transforms;Visual servoing;Frequency-domain analysis;Feature extraction;Visualization;Cameras;Visual servoing;sensor-based control}, + doi={10.1109/LRA.2020.2965027} + @InProceedings{Oliva22c, Author = {Oliva, A. and Spindler, F. and Robuffo Giordano, P. and Chaumette, F.}, Title = {{A Dynamic Simulator for the Franka Emika Robot with Visual-Servoing Enabled Capabilities}}, diff --git a/example/direct-visual-servoing/CMakeLists.txt b/example/direct-visual-servoing/CMakeLists.txt index edff7fef93..868061c77c 100644 --- a/example/direct-visual-servoing/CMakeLists.txt +++ b/example/direct-visual-servoing/CMakeLists.txt @@ -41,6 +41,7 @@ find_package(VISP REQUIRED visp_core visp_robot visp_visual_features visp_vs vis set(example_cpp photometricVisualServoing.cpp + photometricMappingVisualServoing.cpp photometricVisualServoingWithoutVpServo.cpp ) @@ -53,7 +54,9 @@ endforeach() visp_set_source_file_compile_flag(photometricVisualServoing.cpp -Wno-strict-overflow) visp_set_source_file_compile_flag(photometricVisualServoingWithoutVpServo.cpp -Wno-strict-overflow) - +visp_set_source_file_compile_flag(photometricMappingVisualServoing.cpp -Wno-strict-overflow) # Only if dataset found for isolated build visp_add_test(photometricVisualServoing photometricVisualServoing -c -n 20 ${OPTION_TO_DESACTIVE_DISPLAY}) visp_add_test(photometricVisualServoingWithoutVpServo photometricVisualServoingWithoutVpServo -c -n 20 ${OPTION_TO_DESACTIVE_DISPLAY}) +visp_add_test(photometricMappingPCAVisualServoing photometricMappingVisualServoing -c -n 200 -m pca -k 64 -p 100 -l 30.0 ${OPTION_TO_DESACTIVE_DISPLAY}) +visp_add_test(photometricMappingDCTVisualServoing photometricMappingVisualServoing -c -n 200 -m dct -k 64 -l 30.0 ${OPTION_TO_DESACTIVE_DISPLAY}) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp new file mode 100644 index 0000000000..7e50556f0c --- /dev/null +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -0,0 +1,560 @@ +/* + * 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. + */ + +/*! + \example photometricMappingVisualServoing.cpp + + Implemented from \cite Collewet08c, \cite Marchand19a and \cite Marchand20a. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + + +// List of allowed command line options +#define GETOPTARGS "cdi:n:p:m:k:hl:" + +void usage(const char *name, const char *badparam, std::string ipath, int niter, const std::string &method, unsigned numDbImages, const unsigned numComponents, const double lambda); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents, double &lambda); + +/*! + + Print the program options. + + \param name : Program name. + \param badparam : Bad parameter name. + \param ipath : Input image path. + \param niter : Number of iterations. + +*/ +void usage(const char *name, const char *badparam, std::string ipath, int niter, const std::string &method, unsigned numDbImages, const unsigned numComponents, const double lambda) +{ + fprintf(stdout, "\n\ +Visual servoing with compressed photometric features.\n\ +Use either PCA or DCT representations\n\ +\n\ +\n\ +SYNOPSIS\n\ + %s [-i ] [-m pca|dct] [-p ] [-c] [-d] [-n ] [-h]\n", + name); + + fprintf(stdout, "\n\ +OPTIONS: Default\n\ + -i %s\n\ + Set image input path.\n\ + From this path read \"doisneau/doisneau.jpg\"\n\ + images. \n\ + Setting the VISP_INPUT_IMAGE_PATH environment\n\ + variable produces the same behaviour than using\n\ + this option.\n\ + \n\ + -m\n\ + Method to use: either 'PCA' or 'DCT'\n\ + PCA first requires learning a projection from a base of images. see the -p option.\n\ + Default: %s\n\ + -k\n\ + Number of visual servoing features (i.e., PCA or DCT components)\n\ + Default: %d\n\ +\n\ + -p\n\ + Number of images to use to compute PCA. If method is DCT, this option is ignored.\n\ + Default: %d\n\ +\n\ + -c\n\ + Disable the mouse click. Useful to automate the \n\ + execution of this program without human intervention.\n\ +\n\ + -d \n\ + Turn off the display.\n\ +\n\ + -n %%d %d\n\ + Number of visual servoing iterations.\n\ +\n\ + -l %%f %f\n\ + Number of visual servoing iterations.\n\ +\n\ + -h\n\ + Print the help.\n", + ipath.c_str(), method.c_str(), numComponents, numDbImages, niter, lambda); + + if (badparam) + fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); +} +/*! + + Set the program options. + + \param argc : Command line number of parameters. + \param argv : Array of command line parameters. + \param ipath : Input image path. + \param click_allowed : Mouse click activation. + \param display : Display activation. + \param niter : Number of iterations. + + \return false if the program has to be stopped, true otherwise. + +*/ +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, + int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents, double &lambda) +{ + const char *optarg_; + int c; + while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) { + + switch (c) { + case 'c': + click_allowed = false; + break; + case 'd': + display = false; + break; + case 'i': + ipath = optarg_; + break; + case 'm': + method = std::string(optarg_); + break; + case 'p': + numDbImages = atoi(optarg_); + break; + case 'k': + numComponents = atoi(optarg_); + break; + case 'n': + niter = atoi(optarg_); + break; + case 'l': + lambda = atof(optarg_); + break; + case 'h': + usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents, lambda); + return false; + + default: + usage(argv[0], optarg_, ipath, niter, method, numDbImages, numComponents, lambda); + return false; + } + } + + if ((c == 1) || (c == -1)) { + // standalone param or error + usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents, lambda); + std::cerr << "ERROR: " << std::endl; + std::cerr << " Bad argument " << optarg_ << std::endl << std::endl; + return false; + } + + return true; +} + +int main(int argc, const char **argv) +{ +#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + try { + std::string env_ipath; + std::string opt_ipath; + std::string ipath; + std::string filename; + bool opt_click_allowed = true; + bool opt_display = true; + int opt_niter = 400; + std::string opt_method = "dct"; + unsigned opt_numDbImages = 2000; + unsigned opt_numComponents = 32; + double opt_lambda = 5.0; + + double mu = 0.01; // mu = 0 : Gauss Newton ; mu != 0 : LM + double lambdaGN = opt_lambda; + + + + const double Z = 0.8; + const unsigned ih = 240; + const unsigned iw = 320; + const double scenew = 0.6; + const double sceneh = 0.42; + + // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH + // environment variable value + env_ipath = vpIoTools::getViSPImagesDataPath(); + + // Set the default input path + if (!env_ipath.empty()) + ipath = env_ipath; + + // Read the command line options + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter, opt_method, + opt_numDbImages, opt_numComponents, opt_lambda) == false) { + return EXIT_FAILURE; + } + + // Get the option values + if (!opt_ipath.empty()) + ipath = opt_ipath; + + // Compare ipath and env_ipath. If they differ, we take into account + // the input path coming from the command line option + if (!opt_ipath.empty() && !env_ipath.empty()) { + if (ipath != env_ipath) { + std::cout << std::endl << "WARNING: " << std::endl; + std::cout << " Since -i " + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl + << " we skip the environment variable." << std::endl; + } + } + + // Test if an input path is set + if (opt_ipath.empty() && env_ipath.empty()) { + usage(argv[0], nullptr, ipath, opt_niter, opt_method, opt_numDbImages, opt_numComponents, opt_lambda); + std::cerr << std::endl << "ERROR:" << std::endl; + std::cerr << " Use -i option or set VISP_INPUT_IMAGE_PATH " << std::endl + << " environment variable to specify the location of the " << std::endl + << " image path where test images are located." << std::endl + << std::endl; + return EXIT_FAILURE; + } + + vpImage Itexture; + filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); + vpImageIo::read(Itexture, filename); + + vpColVector X[4]; + for (int i = 0; i < 4; i++) + X[i].resize(3); + // Top left corner + X[0][0] = -(scenew / 2.0); + X[0][1] = -(sceneh / 2.0); + X[0][2] = 0; + + // Top right corner + X[1][0] = (scenew / 2.0); + X[1][1] = -(sceneh / 2.0); + X[1][2] = 0; + + // Bottom right corner + X[2][0] = (scenew / 2.0); + X[2][1] = (sceneh / 2.0); + X[2][2] = 0; + + // Bottom left corner + X[3][0] = -(scenew / 2.0); + X[3][1] = (sceneh / 2.0); + X[3][2] = 0; + + vpImageSimulator sim; + + sim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION); + sim.setCleanPreviousImage(true, vpColor::black); + sim.init(Itexture, X); + // ---------------------------------------------------------- + // Create the framegraber (here a simulated image) + vpImage I(ih, iw, 0); + vpImage Irec(ih - vpFeatureLuminance::DEFAULT_BORDER * 2, iw - vpFeatureLuminance::DEFAULT_BORDER * 2, 0); + + vpImage Id; + // camera desired position + vpHomogeneousMatrix cdMo; + cdMo[2][3] = Z; + + + vpCameraParameters cam(870, 870, 160, 120); + std::shared_ptr sMapping = nullptr; + std::shared_ptr sdMapping = nullptr; + + // Setup mapping + if (opt_method == "pca") { + vpUniRand random(17); + std::cout << "Building image database for PCA computation with " << opt_numDbImages << " images" << std::endl; +#if defined(VISP_HAVE_GUI) +#if defined(VISP_HAVE_X11) + vpDisplayX d; +#elif defined(VISP_HAVE_GDI) + vpDisplayGDI d; +#elif defined(VISP_HAVE_GTK) + vpDisplayGTK d; +#elif defined(HAVE_OPENCV_HIGHGUI) + vpDisplayOpenCV d; +#endif + if (opt_display) { + d.init(I, 0, 0, "Image database (subsample)"); + } +#endif + std::vector> images(opt_numDbImages); + for (unsigned i = 0; i < opt_numDbImages; ++i) { + vpColVector to(3, 0.0), positionNoise(3, 0.0); + const double noiseDiv = 16.0; + positionNoise[0] = random.uniform(-scenew / noiseDiv, scenew / noiseDiv); + positionNoise[1] = random.uniform(-sceneh / noiseDiv, sceneh / noiseDiv); + positionNoise[2] = random.uniform(0.0, Z / noiseDiv); + const double noiseDivTo = 16.0; + to[0] = random.uniform(-scenew / noiseDivTo, scenew / noiseDivTo); + to[1] = random.uniform(-sceneh / noiseDivTo, sceneh / noiseDivTo); + const vpColVector from = vpColVector(cdMo.getTranslationVector()) + positionNoise; + vpRotationMatrix Rrot(0.0, 0.0, vpMath::rad(random.uniform(-10, 10))); + vpHomogeneousMatrix dbMo = vpMath::lookAt(from, to, Rrot * vpColVector({ 0.0, 1.0, 0.0 })); + sim.setCameraPosition(dbMo); + sim.getImage(I, cam); + images[i] = I; + if (i % 20 == 0 && opt_display) { + vpDisplay::display(I); + vpDisplay::flush(I); + } + } + std::cout << "Computing PCA, this may take some time!" << std::endl; + // create two distinct objects: if the projection is stateful, using a single mapping could lead to undesired behaviour + vpLuminancePCA pca = vpLuminancePCA::learn(images, opt_numComponents, vpFeatureLuminance::DEFAULT_BORDER); + std::cout << "Explained variance: " << pca.getExplainedVariance().sum() * 100.0 << "%" << std::endl; + sMapping = std::shared_ptr(new vpLuminancePCA(pca)); + sdMapping = std::shared_ptr(new vpLuminancePCA(pca)); + } + else if (opt_method == "dct") { + sMapping = std::shared_ptr(new vpLuminanceDCT(opt_numComponents)); + sdMapping = std::shared_ptr(new vpLuminanceDCT(opt_numComponents)); + } + else { + throw vpException(vpException::badValue, "Method must be pca or dct!"); + } + + // set the robot at the desired position + sim.setCameraPosition(cdMo); + sim.getImage(I, cam); // and aquire the image Id + Id = I; +#if defined(VISP_HAVE_GUI) + // display the image +#if defined(VISP_HAVE_X11) + vpDisplayX d; +#elif defined(VISP_HAVE_GDI) + vpDisplayGDI d; +#elif defined(VISP_HAVE_GTK) + vpDisplayGTK d; +#elif defined(HAVE_OPENCV_HIGHGUI) + vpDisplayOpenCV d; +#endif + +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV) + if (opt_display) { + d.init(I, 20, 10, "Current image"); + vpDisplay::display(I); + vpDisplay::flush(I); + } + if (opt_display && opt_click_allowed) { + std::cout << "Click in the image to continue..." << std::endl; + vpDisplay::getClick(I); + } +#endif +#endif + + // ---------------------------------------------------------- + // position the robot at the initial position + // ---------------------------------------------------------- + + // camera desired position + vpHomogeneousMatrix cMo; + cMo.build(0.0, 0, Z + 0.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(5)); + vpHomogeneousMatrix wMo; // Set to identity + vpHomogeneousMatrix wMc; // Camera position in the world frame + + // set the robot at the desired position + sim.setCameraPosition(cMo); + I = 0; + sim.getImage(I, cam); // and aquire the image Id + +#if defined(VISP_HAVE_GUI) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) + if (opt_display) { + vpDisplay::display(I); + vpDisplay::flush(I); + } + if (opt_display && opt_click_allowed) { + std::cout << "Click in the image to continue..." << std::endl; + vpDisplay::getClick(I); + } +#endif + + vpImage Idiff; + Idiff = I; + + vpImageTools::imageDifference(I, Id, Idiff); + + // Display image difference +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) +#if defined(VISP_HAVE_X11) + vpDisplayX d1, d2; + +#elif defined(VISP_HAVE_GDI) + vpDisplayGDI d1, d2; +#elif defined(VISP_HAVE_GTK) + vpDisplayGTK d1, d2; +#endif + if (opt_display) { + d1.init(Idiff, 40 + static_cast(I.getWidth()), 10, "photometric visual servoing : s-s* "); + d2.init(Irec, 40 + static_cast(I.getWidth()) * 2, 10, "Reconstructed image"); + + vpDisplay::display(Idiff); + vpDisplay::flush(Idiff); + vpDisplay::display(Irec); + vpDisplay::flush(Irec); + } +#endif + // create the robot (here a simulated free flying camera) + vpSimulatorCamera robot; + robot.setSamplingTime(0.04); + wMc = wMo * cMo.inverse(); + robot.setPosition(wMc); + + // ------------------------------------------------------ + // Visual feature, interaction matrix, error + // s, Ls, Lsd, Lt, Lp, etc + // ------------------------------------------------------ + + // current visual feature built from the image + vpFeatureLuminance luminanceI; + luminanceI.init(I.getHeight(), I.getWidth(), Z); + luminanceI.setCameraParameters(cam); + vpFeatureLuminanceMapping sI(luminanceI, sMapping); + sI.build(I); + sI.getMapping()->inverse(sI.get_s(), Irec); + + // desired visual feature built from the image + vpFeatureLuminance luminanceId; + luminanceId.init(I.getHeight(), I.getWidth(), Z); + luminanceId.setCameraParameters(cam); + vpFeatureLuminanceMapping sId(luminanceId, sdMapping); + sId.build(Id); + + // set a velocity control mode + robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); + + int iter = 1; + int iterGN = opt_niter / 8; + double normError = 0; + vpColVector v; // camera velocity sent to the robot + vpColVector error(sI.dimension_s(), 0); + + unsigned int n = 6; + vpMatrix L; + vpMatrix Hs(n, n); + vpMatrix H; + vpMatrix diagHs(n, n); + + vpChrono chrono; + chrono.start(); + std::cout << "Starting VS loop" << std::endl; + do { + std::cout << "--------------------------------------------" << iter++ << std::endl; + + // Acquire the new image + sim.setCameraPosition(cMo); + sim.getImage(I, cam); + vpImageTools::imageDifference(I, Id, Idiff); + + // Compute current visual features + sI.build(I); + sI.getMapping()->inverse(sI.get_s(), Irec); + + if (iter > iterGN) { + mu = 0.0001; + opt_lambda = lambdaGN; + } + sI.interaction(L); + sI.error(sId, error); + + Hs = L.AtA(); + for (unsigned int i = 0; i < n; i++) { + diagHs[i][i] = Hs[i][i]; + } + H = ((mu * diagHs) + Hs).inverseByLU(); + // Compute the control law + v = -opt_lambda * H * L.t() * error; + normError = error.sumSquare(); + + std::cout << " |e| = " << normError << std::endl; + std::cout << " |v| = " << sqrt(v.sumSquare()) << std::endl; + +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) + if (opt_display) { + vpDisplay::display(I); + vpDisplay::flush(I); + vpDisplay::display(Irec); + vpDisplay::flush(Irec); + vpDisplay::display(Idiff); + vpDisplay::flush(Idiff); + } +#endif + + // send the robot velocity + robot.setVelocity(vpRobot::CAMERA_FRAME, v); + wMc = robot.getPosition(); + cMo = wMc.inverse() * wMo; + } while (normError > 200 && iter < opt_niter); + + chrono.stop(); + std::cout << "Time to convergence: " << chrono.getDurationMs() << " ms" << std::endl; + + v = 0; + robot.setVelocity(vpRobot::CAMERA_FRAME, v); + + if (normError > 200) { + return EXIT_FAILURE; + } + return EXIT_SUCCESS; + } + catch (const vpException &e) { + std::cout << "Catch an exception: " << e << std::endl; + return EXIT_FAILURE; + } +#else + (void)argc; + (void)argv; + std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl; + return EXIT_SUCCESS; +#endif +} diff --git a/example/direct-visual-servoing/photometricVisualServoing.cpp b/example/direct-visual-servoing/photometricVisualServoing.cpp index ff189bed98..728aec0a2f 100644 --- a/example/direct-visual-servoing/photometricVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricVisualServoing.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ /*! \example photometricVisualServoing.cpp @@ -440,4 +438,4 @@ int main(int argc, const char **argv) std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl; return EXIT_SUCCESS; #endif - } +} diff --git a/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp b/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp index 023bf73a84..c0be3a104b 100644 --- a/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp +++ b/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ /*! \example photometricVisualServoingWithoutVpServo.cpp @@ -491,4 +489,4 @@ int main(int argc, const char **argv) std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl; return EXIT_SUCCESS; #endif - } +} diff --git a/modules/visual_features/CMakeLists.txt b/modules/visual_features/CMakeLists.txt index 2e5a28af62..0789619ec8 100644 --- a/modules/visual_features/CMakeLists.txt +++ b/modules/visual_features/CMakeLists.txt @@ -33,12 +33,17 @@ # ############################################################################# -vp_add_module(visual_features visp_core OPTIONAL visp_blob visp_me) +vp_add_module(visual_features visp_core OPTIONAL visp_blob visp_me visp_io) vp_glob_module_sources() vp_module_include_directories() vp_create_module() vp_add_tests() +if(WITH_CATCH2) + # catch2 is private + include_directories(${CATCH2_INCLUDE_DIRS}) +endif() + if(USE_OPENCV) vp_set_source_file_compile_flag(src/feature-builder/vpFeatureBuilderEllipse.cpp -Wno-float-equal) vp_set_source_file_compile_flag(src/feature-builder/vpFeatureBuilderLine.cpp -Wno-float-equal) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h index b857262786..eaeb1e88c1 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h @@ -113,6 +113,8 @@ class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature void error(const vpBasicFeature &s_star, vpColVector &e); double get_Z() const; + unsigned int getBorder() const; + void init(unsigned int _nbr, unsigned int _nbc, double _Z); @@ -124,9 +126,11 @@ class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature vpFeatureLuminance &operator=(const vpFeatureLuminance &f); - void setCameraParameters(vpCameraParameters &_cam); + void setCameraParameters(const vpCameraParameters &_cam); void set_Z(double Z); + static const int DEFAULT_BORDER; + public: vpCameraParameters cam; }; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h new file mode 100644 index 0000000000..ae7573bcc5 --- /dev/null +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -0,0 +1,420 @@ +/* + * 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: + * Luminance based feature. + */ + +#ifndef VP_FEATURE_LUMINANCE_MAPPING_H +#define VP_FEATURE_LUMINANCE_MAPPING_H +#include +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#include +#include + +#include +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +/*! + * \brief Base class for functions that map an image and its interaction matrix to a different domain. + * + * \ingroup group_visual_features + * + * - The mapping\f$ \mathbf{I} \rightarrow \mathbf{z}\f$ is done via vpLuminanceMapping::map + * - The projection of the interaction matrix \f$ \mathbf{L_I} \rightarrow \mathbf{L_z}\f$ is performed in vpLuminanceMapping::interaction + * - If possible the inverse mapping (i.e., image reconstruction) is available throug vpLuminanceMapping::inverse +*/ +class VISP_EXPORT vpLuminanceMapping +{ +public: + /** + * \brief Construct a new vp Luminance Mapping object + * + * @param mappingSize The size of the space that this transformation maps to. + */ + vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } + + /** + * \brief Map an image \p I to a representation \p s. + * This representation s has getProjectionSize() rows. + * + * Note that when combined with vpFeatureLuminanceMapping, + * The image \p I does not have the same size as the image input of vpFeatureLuminanceMapping::build. + * \p I is the center crop of this image. + * @param I The input image + * @param s The resulting representation that will serve as visual servoing features. + */ + virtual void map(const vpImage &I, vpColVector &s) = 0; + + /** + * \brief Compute the interaction matrix associated with the representation \p s + * + * @param I input image used to compute s + * @param LI Photometric interaction matrix associated to \p I (see vpFeatureLuminance) + * @param s the already computed representation + * @param L The output interaction matrix, of dimensions getProjectionSize() x 6 + */ + virtual void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) = 0; + + /** + * \brief Reconstruct \p I from a representation \p s + * + * @param s the representation + * @param I Output lossy reconstruction + */ + virtual void inverse(const vpColVector &s, vpImage &I) = 0; + + /** + * \brief Returns the size of the space to which an image is mapped to. + * + * @return space size + */ + unsigned int getProjectionSize() const { return m_mappingSize; } + + /** + * \brief Returns the number of pixels that are removed by the photometric VS computation + * + * @return space size + */ + unsigned int getBorder() const { return m_border; } + + /** + * \brief Set the number of pixels that are removed by the photometric VS computation + * This function should be called by vpFeatureLuminanceMapping + * + * @param border + */ + void setBorder(unsigned border) { m_border = border; } + + static void imageAsVector(const vpImage &I, vpColVector &Ivec, unsigned border); + static void imageAsMatrix(const vpImage &I, vpMatrix &Imat, unsigned border); + +protected: + unsigned m_mappingSize; //! Final vector size + unsigned m_border; //! Borders that were removed during raw photometric VS computation +}; + +/** + * \brief Implementation of \cite Marchand19a. + * + * \ingroup group_visual_features + * + * Projects an image onto an orthogonal subspace, + * obtained via Principal Component Analysis (PCA). + * + * The orthogonal basis is obtained through Singular Value Decomposition of a dataset of images (see vpLuminancePCA::learn) + * where the \f$ k \f$ first basis vectors that explain the most variance are kept. + * + * an image \f$ I \f$ is projected to the representation \f$ \mathbf{s} \f$ with: + * \f[ \mathbf{s} = \mathbf{U}^\top (vec(\mathbf{I}) - vec(\mathbf{\bar I})) \f] + * + * with \f$ \mathbf{U} \f$ the subspace projection matrix (\f$ dim(\mathbf{I}) \times k \f$) and \f$ \mathbf{\bar I} \f$ is the average image computed from the dataset. + * + * + */ +class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping +{ +public: + vpLuminancePCA() : vpLuminanceMapping(0), m_basis(nullptr), m_mean(nullptr), m_Ivec(0), m_Ih(0), m_Iw(0) { } + + /** + * \brief Build a new PCA object + * + * @param basis \f$ \mathbf{U}^\top \f$ a k x dim(I) matrix + * @param mean \f$ vec(\mathbf{\bar I}) \f$ the mean image represented as a vector + * @param explainedVariance The explained variance for each of the k vectors. + */ + vpLuminancePCA(const std::shared_ptr &basis, const std::shared_ptr &mean, const vpColVector &explainedVariance); + + /** + * \brief Copy constructor: does not make a deep copy of the basis and mean + */ + vpLuminancePCA(const vpLuminancePCA &other); + + vpLuminancePCA &operator=(const vpLuminancePCA &other); + + /** + * \brief Initialize the PCA object with a basis, mean and explained variance vector + * + * \sa vpLuminancePCA() + * @param basis + * @param mean + * @param variance + */ + void init(const std::shared_ptr &basis, const std::shared_ptr &mean, const vpColVector &variance); + + /** + * \brief Get \f$ \mathbf{U}^\top \f$, the subspace projection matrix (\f$ k \times dim(\mathbf{I}) \f$) + * + * @return std::shared_ptr + */ + std::shared_ptr getBasis() const { return m_basis; } + /** + * \brief Get \f$ vec(\mathbf{\bar I}) \f$, the mean image computed from the dataset. + * @return std::shared_ptr + */ + std::shared_ptr getMean() const { return m_mean; } + + /** + * \brief Get the values of explained variance by each of the eigen vectors. + * + * When all eigenvectors of the dataset are considered, the explained variance total is 1. + * When they are not all considered (as should be the case), their sum should be below 1. + * @return vpColVector + */ + vpColVector getExplainedVariance() const { return m_explainedVariance; } + + void map(const vpImage &I, vpColVector &s) vp_override; + void inverse(const vpColVector &s, vpImage &I) vp_override; + void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) vp_override; + + /** + * \brief Save the PCA basis to multiple text files, for later use via the \ref load function. + * + * @param basisFilename The file in which \f$ \mathbf{U}^\top \f$ is stored + * @param meanFileName The file in which \f$ \mathbf{\bar I} \f$ is stored + * @param explainedVarianceFile The file containing the explained variance. + * + * \throws if the basis is null or mean is null + */ + void save(const std::string &basisFilename, const std::string &meanFileName, const std::string &explainedVarianceFile) const; + + /** + * \brief Save the PCA basis to multiple text files, for later use via the \ref load function. + * + * @param basisFilename The file in which \f$ \mathbf{U}^\top \f$ is stored + * @param meanFileName The file in which \f$ \mathbf{\bar I} \f$ is stored + * @param explainedVarianceFile The file containing the explained variance. + * + * \throws if files cannot be read, or if basis and mean dimensions are incorrect. + */ + static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName, const std::string &explainedVarianceFile); + +#ifdef VISP_HAVE_MODULE_IO + /** + * \brief Compute a new Principal Component Analysis on set of images, stored on disk. + * + * @param imageFiles The list of image paths to load and use to compute the PCA + * @param projectionSize the number of eigenvectors that are kept for the final projection + * @param imageBorder The number of pixels to crop on each side of the image before adding it to the image set, effectively taking the center crop. + * Useful when the stored images do not have the correct dimensions + * @return the PCA computed on the imageFiles + * + * \throws if the images do not have the same dimensions + */ + static vpLuminancePCA learn(const std::vector &imageFiles, const unsigned int projectionSize, const unsigned int imageBorder = 0); +#endif + + /** + * \brief Compute a new Principal Component Analysis on set of images. + * + * @param images The list of images used to compute the PCA + * @param projectionSize the number of eigenvectors that are kept for the final projection + * @param imageBorder The number of pixels to crop on each side of the image before adding it to the image set, effectively taking the center crop. + * Useful when the images do not have the correct dimensions. Typically, the input image to PCA is smaller than the one used when computing luminance features, since the latter step requires crops the image. + * @return the PCA computed on the images + * + * \throws if the images do not have the same dimensions + */ + static vpLuminancePCA learn(const std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder = 0); + /** + * \brief Compute a new Principal Component Analysis on dataset + * + * @param images The data matrix, where each column represents a single data point (image) + * @param projectionSize the number of eigenvectors that are kept for the final projection + * @return the PCA computed on the images + */ + static vpLuminancePCA learn(const vpMatrix &images, const unsigned int projectionSize); + + +private: + std::shared_ptr m_basis; //! \f$ \mathbf{U}^\top \f$ a K by dim(I) orthogonal matrix + std::shared_ptr m_mean; //! \f$ \mathbf{\bar I} \f$ The mean image + vpColVector m_explainedVariance; //! The explained variance + vpColVector m_Ivec; //! Vector representation of the image + unsigned int m_Ih, m_Iw; //! Input image dimensions (without borders); +}; + +/** + * \brief Implementation of \cite Marchand20a. + * + * \ingroup group_visual_features + * + * Computes the Discrete Cosine Transform (DCT) representation of the image. + * Only the K first components are preserved and stored into a vector when calling map. These components correspond to the lowest frequencies of the input image. + */ +class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping +{ +public: + /** + * \brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. + * + */ + class VISP_EXPORT vpMatrixZigZagIndex + { + public: + vpMatrixZigZagIndex(); + /** + * \brief Initalize the ZigZag object. Computes and stores the zigzag indexing for a given matrix size + * + * @param rows the matrix's number of rows + * @param cols the matrix's number of cols + */ + void init(unsigned rows, unsigned cols); + /** + * \brief Fill the vector s with (end - start) values, according to the zigzag matrix indexing strategy + * + * @param m the matrix + * @param start The first value. Use 0 to start with the matrix's top left value + * @param end The last value to store in the vector. (exclusive) + * @param s The vector in which to store the values + */ + void getValues(const vpMatrix &m, unsigned int start, unsigned int end, vpColVector &s) const; + + /** + * \brief set the values in the matrix, according to the values stored in the vector s and the zigzag indexing strategy + * + * @param s The vector from which to set the values + * @param start the zigzag index at which to start filling values + * @param m The matrix in which the values will be replaced + */ + void setValues(const vpColVector &s, unsigned int start, vpMatrix &m) const; + + private: + std::vector m_rowIndex; // Contains the row index of the nth value of the zigzag indexing + std::vector m_colIndex; // Contains the row index of the nth value of the zigzag indexing + unsigned m_rows; + unsigned m_cols; + }; + + /** + * \brief Build a new DCT object + * + * @param k the number of components to keep from the DCT matrix and use as servoing features + */ + vpLuminanceDCT(const unsigned int k) : vpLuminanceMapping(k) + { + init(k); + } + + /** + * \brief Initialize the DCT object with the number of required components + */ + void init(const unsigned int k) + { + m_mappingSize = k; + m_border = vpFeatureLuminance::DEFAULT_BORDER; + m_Ih = m_Iw = 0; + } + + /** + * \brief Copy constructor + */ + vpLuminanceDCT(const vpLuminanceDCT &other); + + vpLuminanceDCT &operator=(const vpLuminanceDCT &other) = default; + + void map(const vpImage &I, vpColVector &s) vp_override; + void inverse(const vpColVector &s, vpImage &I) vp_override; + void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) vp_override; + +private: + void computeDCTMatrix(vpMatrix &D, unsigned int n) const; + void computeDCTMatrices(unsigned int rows, unsigned int cols); + +protected: + unsigned m_Ih, m_Iw; //! image dimensions (without borders) + vpMatrix m_Imat; //! Image as a matrix + vpMatrix m_dct; //! DCT representation of the image + vpMatrix m_Dcols, m_Drows; //! the computed DCT matrices. The separable property of DCt is used so that a 1D DCT is computed on rows and another on columns of the result of the first dct; + std::array m_dIdrPlanes; //! Luminance interaction matrix, seen as six image planes + vpLuminanceDCT::vpMatrixZigZagIndex m_zigzag; //! zigzag indexing helper +}; + +/** + * \brief Class to combine luminance features (photometric servoing) + * + * \ingroup group_visual_features + * + * with a mapping \f$ f(\mathbf{I}) \f$ that projects an image to a low dimensional representation \f$ \mathbf{s} \f$ (see vpLuminanceMapping::map). + * The interaction matrix of \f$ \mathbf{s} \f$ is computed as a function of \f$ \mathbf{I}, \mathbf{L_I} \f$ (see vpLuminanceMapping::interaction) + * + * The mapping \f$ f \f$ is applied to the center crop of the image, + * where the interaction matrix of the pixels can be computed (see vpFeatureLuminance::getBorder). + * + * \see vpLuminanceDCT, vpLuminancePCA, vpFeatureLuminance + */ +class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature +{ +public: + vpFeatureLuminanceMapping(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const std::shared_ptr mapping); + vpFeatureLuminanceMapping(const vpFeatureLuminance &luminance, std::shared_ptr mapping); + void init() vp_override; + void init(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, std::shared_ptr mapping); + void init(const vpFeatureLuminance &luminance, std::shared_ptr mapping); + + vpFeatureLuminanceMapping(const vpFeatureLuminanceMapping &f); + vpFeatureLuminanceMapping &operator=(const vpFeatureLuminanceMapping &f); + vpFeatureLuminanceMapping *duplicate() const vp_override; + + virtual ~vpFeatureLuminanceMapping() = default; + + void build(vpImage &I); + + void display(const vpCameraParameters &, const vpImage &, const vpColor & = vpColor::green, + unsigned int = 1) const vp_override + { } + void display(const vpCameraParameters &, const vpImage &, const vpColor & = vpColor::green, + unsigned int = 1) const vp_override + { } + + vpColVector error(const vpBasicFeature &s_star, unsigned int select = FEATURE_ALL) vp_override; + void error(const vpBasicFeature &s_star, vpColVector &e); + + vpMatrix interaction(unsigned int select = FEATURE_ALL) vp_override; + void interaction(vpMatrix &L); + + void print(unsigned int select = FEATURE_ALL) const vp_override; + + vpFeatureLuminance &getLuminanceFeature() { return m_featI; } + std::shared_ptr &getMapping() { return m_mapping; } + +private: + std::shared_ptr m_mapping; + vpFeatureLuminance m_featI; + vpMatrix m_LI; //! Photometric interaction matrix + vpImage I; +}; +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index e2560d33d9..3d281d5239 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Luminance feature. - * -*****************************************************************************/ + */ /*! \file vpFeatureLuminance.cpp @@ -52,6 +50,8 @@ BEGIN_VISP_NAMESPACE +const int vpFeatureLuminance::DEFAULT_BORDER = 10; + /*! Initialize the memory space requested for vpFeatureLuminance visual feature. */ @@ -97,26 +97,29 @@ void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z) /*! Default constructor that build a visual feature. */ -vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixInfo(nullptr), firstTimeIn(0), cam() +vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam() { nbParameters = 1; dim_s = 0; + if (flags != nullptr) { + delete[] flags; + } flags = nullptr; init(); } /*! - Copy constructor. + Copy constructor. */ vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance &f) - : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(nullptr), firstTimeIn(0), cam() + : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam() { *this = f; } /*! - Copy operator. + Copy operator. */ vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance &f) { @@ -126,11 +129,13 @@ vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance &f) bord = f.bord; firstTimeIn = f.firstTimeIn; cam = f.cam; + dim_s = f.dim_s; if (pixInfo) delete[] pixInfo; pixInfo = new vpLuminance[dim_s]; for (unsigned int i = 0; i < dim_s; i++) pixInfo[i] = f.pixInfo[i]; + s.resize(dim_s); return (*this); } @@ -163,8 +168,9 @@ void vpFeatureLuminance::set_Z(double Z_) \return The value of \f$ Z \f$. */ double vpFeatureLuminance::get_Z() const { return Z; } +unsigned int vpFeatureLuminance::getBorder() const { return bord; } -void vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam) { cam = _cam; } +void vpFeatureLuminance::setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; } #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS /*! @@ -194,9 +200,9 @@ vpFeatureLuminance &vpFeatureLuminance::build(vpImage &I) if (firstTimeIn == 0) { firstTimeIn = 1; l = 0; - for (unsigned int i = bord; i < (nbr - bord); ++i) { - // cout << i << endl ; - for (unsigned int j = bord; j < (nbc - bord); ++j) { + for (unsigned int i = bord; i < nbr - bord; i++) { + for (unsigned int j = bord; j < nbc - bord; j++) { + double x = 0, y = 0; vpPixelMeterConversion::convertPoint(cam, j, i, x, y); @@ -210,6 +216,7 @@ vpFeatureLuminance &vpFeatureLuminance::build(vpImage &I) } l = 0; + for (unsigned int i = bord; i < (nbr - bord); ++i) { for (unsigned int j = bord; j < (nbc - bord); ++j) { Ix = px * vpImageFilter::derivativeFilterX(I, i, j); @@ -228,7 +235,6 @@ vpFeatureLuminance &vpFeatureLuminance::build(vpImage &I) } /*! - Compute and return the interaction matrix \f$ L_I \f$. The computation is made thanks to the values of the luminance features \f$ I \f$ */ @@ -370,8 +376,3 @@ vpFeatureLuminance *vpFeatureLuminance::duplicate() const } END_VISP_NAMESPACE -/* - * Local variables: - * c-basic-offset: 2 - * End: - */ diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp new file mode 100644 index 0000000000..e167e6d8d7 --- /dev/null +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -0,0 +1,601 @@ +/* + * 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: + * Luminance dimensionality reduction features + */ + +#include +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifdef VISP_HAVE_MODULE_IO +#include +#endif +BEGIN_VISP_NAMESPACE + +// vpLuminanceMapping + +void vpLuminanceMapping::imageAsVector(const vpImage &I, vpColVector &Ivec, unsigned border) +{ + const unsigned h = I.getHeight(); + const unsigned w = I.getWidth(); + if (h < 2 * border || w < 2 * border) { + throw vpException(vpException::dimensionError, "Image is smaller than required border crop"); + } + Ivec.resize((h - 2 * border) * (w - 2 * border)); + unsigned l = 0; + for (unsigned i = border; i < h - border; ++i) { + for (unsigned j = border; j < w - border; ++j) { + Ivec[l++] = (double)I[i][j]; + } + } +} + +void vpLuminanceMapping::imageAsMatrix(const vpImage &I, vpMatrix &Imat, unsigned border) +{ + const unsigned h = I.getHeight(); + const unsigned w = I.getWidth(); + if (h < 2 * border || w < 2 * border) { + throw vpException(vpException::dimensionError, "Image is smaller than required border crop"); + } + Imat.resize((h - 2 * border), (w - 2 * border), false, false); + for (unsigned i = border; i < h - border; ++i) { + for (unsigned j = border; j < w - border; ++j) { + Imat[i - border][j - border] = (double)I[i][j]; + } + } +} + +// vpLuminancePCA + +vpLuminancePCA::vpLuminancePCA(const std::shared_ptr &basis, const std::shared_ptr &mean, const vpColVector &explainedVariance) + : vpLuminanceMapping(basis->getRows()) +{ + m_Ih = m_Iw = 0; + init(basis, mean, explainedVariance); +} + +void vpLuminancePCA::init(const std::shared_ptr &basis, const std::shared_ptr &mean, const vpColVector &variance) +{ + if (basis->getCols() != mean->getRows()) { + throw vpException(vpException::dimensionError, "PCA mean and basis should have the same number of inputs"); + } + if (variance.getRows() != basis->getRows()) { + throw vpException(vpException::dimensionError, "PCA explained variance should have the same size as the subspace"); + } + m_mappingSize = basis->getRows(); + m_basis = basis; + m_mean = mean; + m_explainedVariance = variance; + m_border = vpFeatureLuminance::DEFAULT_BORDER; +} + +vpLuminancePCA::vpLuminancePCA(const vpLuminancePCA &other) : vpLuminanceMapping(other.m_mappingSize) +{ + *this = other; +} + +vpLuminancePCA &vpLuminancePCA::operator=(const vpLuminancePCA &other) +{ + m_basis = other.m_basis; + m_mean = other.m_mean; + m_explainedVariance = other.m_explainedVariance; + m_mappingSize = other.m_mappingSize; + m_border = other.m_border; + m_Ivec = other.m_Ivec; + m_Ih = other.m_Ih; + m_Iw = other.m_Iw; + + return *this; +} + +void vpLuminancePCA::map(const vpImage &I, vpColVector &s) +{ + m_Ih = I.getHeight() - 2 * m_border; + m_Iw = I.getWidth() - 2 * m_border; + imageAsVector(I, m_Ivec, m_border); + + m_Ivec -= *m_mean; + s = (*m_basis) * m_Ivec; +} + +void vpLuminancePCA::inverse(const vpColVector &s, vpImage &I) +{ + const vpColVector vI = ((*m_basis).transpose() * s + (*m_mean)); + I.resize(m_Ih, m_Iw); + // Vector to image + for (unsigned int i = 0; i < m_Ih; ++i) { + for (unsigned int j = 0; j < m_Iw; ++j) { + I[i][j] = static_cast(vI[i * m_Iw + j]); + } + } +} + +void vpLuminancePCA::interaction(const vpImage &, const vpMatrix &LI, const vpColVector &, vpMatrix &L) +{ + L = (*m_basis) * LI; +} + +vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std::string &meanFilename, const std::string &explainedVarianceFile) +{ + std::shared_ptr basis = std::make_shared(); + std::shared_ptr mean = std::make_shared(); + vpColVector explainedVariance; + vpMatrix::loadMatrix(basisFilename, *basis, true); + vpMatrix::loadMatrix(meanFilename, *mean, true); + vpMatrix::loadMatrix(explainedVarianceFile, explainedVariance, true); + + if (mean->getCols() > 1) { + throw vpException(vpException::dimensionError, + "Read something that was not a column vector when trying to load the PCA mean vector"); + } + if (explainedVariance.getCols() > 1) { + throw vpException(vpException::dimensionError, + "Read something that was not a column vector when trying to load the PCA components explained variance"); + } + if (basis->getCols() != mean->getRows()) { + std::stringstream ss; + ss << "Error when loading PCA from binary files"; + ss << "The basis matrix had dimensions (" << basis->getRows() << ", " << basis->getCols() << ")"; + ss << " and the mean vector had size " << mean->getRows() << "."; + ss << "You may be loading data from two different PCAs"; + throw vpException(vpException::dimensionError, ss.str()); + } + + return vpLuminancePCA(basis, mean, explainedVariance); +} + +void vpLuminancePCA::save(const std::string &basisFilename, const std::string &meanFilename, const std::string &explainedVarianceFile) const +{ + if (m_basis.get() == nullptr || m_mean.get() == nullptr) { + throw vpException(vpException::notInitialized, + "Tried to save a PCA projection that was uninitialized"); + } + if (m_basis->size() == 0 || m_mean->getCols() == 0 || m_basis->getCols() != m_mean->getRows()) { + throw vpException(vpException::dimensionError, + "Tried to save a PCA projection but there are issues with the basis and mean dimensions"); + } + vpMatrix::saveMatrix(basisFilename, *m_basis, true); + vpMatrix::saveMatrix(meanFilename, *m_mean, true); + vpMatrix::saveMatrix(explainedVarianceFile, m_explainedVariance, true); +} + +vpLuminancePCA vpLuminancePCA::learn(const std::vector> &images, const unsigned int projectionSize, const unsigned int border) +{ + vpMatrix matrix; + for (unsigned i = 0; i < images.size(); ++i) { + const vpImage &I = images[i]; + if (i == 0) { + matrix.resize(images.size(), (I.getHeight() - 2 * border) * (I.getWidth() - 2 * border)); + } + if ((I.getHeight() - 2 * border) * (I.getWidth() - 2 * border) != matrix.getCols()) { + throw vpException(vpException::badValue, "Not all images have the same dimensions when learning pca"); + } + for (unsigned j = border; j < I.getHeight() - border; ++j) { + for (unsigned k = border; k < I.getWidth() - border; ++k) { + matrix[i][(j - border) * (I.getWidth() - 2 * border) + k - border] = I[j][k]; + } + } + } + + return vpLuminancePCA::learn(matrix.transpose(), projectionSize); +} + +#ifdef VISP_HAVE_MODULE_IO +vpLuminancePCA vpLuminancePCA::learn(const std::vector &imageFiles, const unsigned int projectionSize, const unsigned int border) +{ + vpMatrix matrix; + vpImage I; + for (unsigned i = 0; i < imageFiles.size(); ++i) { + vpImageIo::read(I, imageFiles[i]); + if (i == 0) { + matrix.resize(imageFiles.size(), (I.getHeight() - 2 * border) * (I.getWidth() - 2 * border)); + } + if ((I.getHeight() - 2 * border) * (I.getWidth() - 2 * border) != matrix.getCols()) { + throw vpException(vpException::badValue, "Not all images have the same dimensions when learning pca"); + } + for (unsigned j = border; j < I.getHeight() - border; ++j) { + for (unsigned k = border; k < I.getWidth() - border; ++k) { + matrix[i][(j - border) * (I.getWidth() - 2 * border) + k - border] = I[j][k]; + } + } + } + return vpLuminancePCA::learn(matrix.transpose(), projectionSize); +} +#endif + +vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int projectionSize) +{ + if (projectionSize > images.getRows() || projectionSize > images.getCols()) { + throw vpException(vpException::badValue, "Cannot use a subspace greater than the data dimensions (number of pixels or images)"); + } + if (images.getRows() < images.getCols()) { + throw vpException(vpException::badValue, "Cannot compute SVD when there are more images (columns) than pixels (rows)"); + } + // Mean computation + vpColVector mean(images.getRows(), 0.0); + for (unsigned i = 0; i < images.getCols(); ++i) { + mean += images.getCol(i); + } + mean /= images.getCols(); + + // Before SVD, center data + vpMatrix centered(images.getRows(), images.getCols()); + for (unsigned i = 0; i < centered.getRows(); ++i) { + for (unsigned j = 0; j < centered.getCols(); ++j) { + centered[i][j] = images[i][j] - mean[i]; + } + } + + vpColVector eigenValues; + vpMatrix V; + centered.svd(eigenValues, V); + vpMatrix U(centered.getRows(), projectionSize); + for (unsigned i = 0; i < centered.getRows(); ++i) { + for (unsigned j = 0; j < projectionSize; ++j) { + U[i][j] = centered[i][j]; + } + } + double cumEigenValues = eigenValues.sum(); + vpColVector componentsExplainedVar(eigenValues, 0, projectionSize); + componentsExplainedVar /= cumEigenValues; + std::shared_ptr basis = std::make_shared(U.t()); + std::shared_ptr meanPtr = std::make_shared(mean); + return vpLuminancePCA(basis, meanPtr, componentsExplainedVar); +} + +//vpMatrixZigZagIndex +vpLuminanceDCT::vpMatrixZigZagIndex::vpMatrixZigZagIndex() { } + +void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) +{ + // Adapted from https://www.geeksforgeeks.org/print-matrix-in-zig-zag-fashion/ + m_colIndex.resize(rows * cols); + m_rowIndex.resize(rows * cols); + m_rows = rows; + m_cols = cols; + int rowCount = static_cast(rows); + int colCount = static_cast(cols); + + unsigned int index = 0; + int row = 0, col = 0; + + bool row_inc = 0; + + int mindim = std::min(rowCount, colCount); + for (int len = 1; len <= mindim; ++len) { + for (int i = 0; i < len; ++i) { + m_rowIndex[index] = row; + m_colIndex[index] = col; + ++index; + if (i + 1 == len) { + break; + } + + if (row_inc) { + ++row; + --col; + } + else { + --row; + ++col; + } + } + + if (len == mindim) { + break; + } + + if (row_inc) + ++row, row_inc = false; + else + ++col, row_inc = true; + } + + // Update the indexes of row and col variable + if (row == 0) { + if (col == rowCount - 1) { + ++row; + } + else { + ++col; + } + row_inc = 1; + } + else { + if (row == colCount - 1) { + ++col; + } + else { + ++row; + } + row_inc = 0; + } + + // Print the next half zig-zag pattern + int maxdim = std::max(rowCount, rowCount) - 1; + for (int len, diag = maxdim; diag > 0; --diag) { + + if (diag > mindim) { + len = mindim; + } + else { + len = diag; + } + + for (int i = 0; i < len; ++i) { + m_rowIndex[index] = row; + m_colIndex[index] = col; + ++index; + + if (i + 1 == len) { + break; + } + + if (row_inc) { + ++row; + --col; + } + else { + ++col; + --row; + } + } + + if (row == 0 || col == rowCount - 1) { + if (col == rowCount - 1) { + ++row; + } + else { + ++col; + } + row_inc = true; + } + + else if (col == 0 || row == colCount - 1) { + if (row == colCount - 1) { + ++col; + } + else { + ++row; + } + row_inc = false; + } + } +} + +void vpLuminanceDCT::vpMatrixZigZagIndex::getValues(const vpMatrix &m, unsigned int start, unsigned int end, vpColVector &s) const +{ + if (m.getRows() != m_rows || m.getCols() != m_cols) { + throw vpException(vpException::dimensionError, "Input matrix has wrong dimensions"); + } + + if (end <= start) { + throw vpException(vpException::dimensionError, "End index should be > to the start index"); + } + + s.resize(end - start, false); + + for (unsigned index = start; index < end; ++index) { + s[index - start] = m[m_rowIndex[index]][m_colIndex[index]]; + } +} + +void vpLuminanceDCT::vpMatrixZigZagIndex::setValues(const vpColVector &s, unsigned int start, vpMatrix &m) const +{ + if (m.getRows() != m_rows || m.getCols() != m_cols) { + throw vpException(vpException::dimensionError, "Input matrix has wrong dimensions"); + } + + if (start + s.size() > m.size()) { + throw vpException(vpException::dimensionError, "Start index combined to vector size exceeds matrix size"); + } + + for (unsigned index = start; index < start + s.size(); ++index) { + m[m_rowIndex[index]][m_colIndex[index]] = s[index - start]; + } +} + +// vpLuminanceDCT + +vpLuminanceDCT::vpLuminanceDCT(const vpLuminanceDCT &other) : vpLuminanceMapping(other.getProjectionSize()) +{ + *this = other; +} + +void vpLuminanceDCT::map(const vpImage &I, vpColVector &s) +{ + m_Ih = I.getHeight() - 2 * m_border; + m_Iw = I.getWidth() - 2 * m_border; + if (m_Imat.getCols() != m_Ih || m_Imat.getRows() != m_Iw) { + computeDCTMatrices(m_Ih, m_Iw); + m_zigzag.init(m_Ih, m_Iw); + } + imageAsMatrix(I, m_Imat, m_border); + m_dct = m_Dcols * m_Imat * m_Drows; + m_zigzag.getValues(m_dct, 0, m_mappingSize, s); +} + +void vpLuminanceDCT::computeDCTMatrix(vpMatrix &D, unsigned int n) const +{ + D.resize(n, n, false, false); + for (unsigned i = 0; i < n; i++) { + D[0][i] = 1.0 / sqrt(n); + } + double alpha = sqrt(2./(n)); + for (unsigned int i = 1; i < n; i++) { + for (unsigned int j = 0; j < n; j++) { + D[i][j] = alpha*cos((2 * j + 1) * i * M_PI / (2.0 * n)); + } + } +} + +void vpLuminanceDCT::computeDCTMatrices(unsigned int rows, unsigned int cols) +{ + computeDCTMatrix(m_Dcols, rows); + computeDCTMatrix(m_Drows, cols); + m_Drows = m_Drows.transpose(); +} + +void vpLuminanceDCT::inverse(const vpColVector &s, vpImage &I) +{ + vpMatrix dctCut(m_dct.getRows(), m_dct.getCols(), 0.0); + m_zigzag.setValues(s, 0, dctCut); + const vpMatrix Ir = m_Dcols.t() * dctCut * m_Drows.t(); + I.resize(Ir.getRows(), Ir.getCols()); + for (unsigned int i = 0; i < I.getRows(); ++i) { + for (unsigned int j = 0; j < I.getCols(); ++j) { + I[i][j] = std::max(0.0, std::min(Ir[i][j], 255.0)); + } + } +} + +void vpLuminanceDCT::interaction(const vpImage &, const vpMatrix &LI, const vpColVector &, vpMatrix &L) +{ + const vpMatrix LIT = LI.t(); + for (unsigned int dof = 0; dof < 6; ++dof) { + m_dIdrPlanes[dof].resize(m_Ih, m_Iw, false, false); + memcpy(m_dIdrPlanes[dof].data, LIT[dof], m_Ih * m_Iw * sizeof(double)); + } + + L.resize(m_mappingSize, 6, false, false); + vpMatrix dTddof(m_Ih, m_Iw); + vpColVector column; + for (unsigned int dof = 0; dof < 6; ++dof) { + dTddof = m_Dcols * m_dIdrPlanes[dof] * m_Drows; + m_zigzag.getValues(dTddof, 0, m_mappingSize, column); + for (unsigned int row = 0; row < L.getRows(); ++row) { + L[row][dof] = column[row]; + } + } +} + +// Feature luminance mapping + +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpCameraParameters &cam, +unsigned int h, unsigned int w, double Z, std::shared_ptr mapping) +{ + init(cam, h, w, Z, mapping); +} + +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminance &luminance, std::shared_ptr mapping) +{ + init(luminance, mapping); +} +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminanceMapping &f) : vpBasicFeature() +{ + *this = f; +} + +void vpFeatureLuminanceMapping::init() +{ + dim_s = 0; + m_featI.init(0, 0, 0.0); + m_mapping = nullptr; +} + +void vpFeatureLuminanceMapping::init( + const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, + std::shared_ptr mapping) +{ + m_featI.init(h, w, Z); + m_featI.setCameraParameters(cam); + m_mapping = mapping; + m_mapping->setBorder(m_featI.getBorder()); + dim_s = m_mapping->getProjectionSize(); + s.resize(dim_s, true); +} +void vpFeatureLuminanceMapping::init(const vpFeatureLuminance &luminance, std::shared_ptr mapping) +{ + m_featI = luminance; + m_mapping = mapping; + dim_s = m_mapping->getProjectionSize(); + m_mapping->setBorder(m_featI.getBorder()); + s.resize(dim_s, true); +} + +vpFeatureLuminanceMapping &vpFeatureLuminanceMapping::operator=(const vpFeatureLuminanceMapping &f) +{ + dim_s = f.dim_s; + s = f.s; + m_mapping = f.m_mapping; + m_featI = f.m_featI; + return *this; +} +vpFeatureLuminanceMapping *vpFeatureLuminanceMapping::duplicate() const +{ + return new vpFeatureLuminanceMapping(*this); +} + +void vpFeatureLuminanceMapping::build(vpImage &I) +{ + m_featI.build(I); + m_featI.interaction(m_LI); + m_mapping->map(I, s); +} + +vpColVector vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, unsigned int select) +{ + if (select != FEATURE_ALL) { + throw vpException(vpException::notImplementedError, "cannot compute error on subset of a mapping"); + } + vpColVector e(dim_s); + error(s_star, e); + return e; +} + +void vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, vpColVector &e) +{ + e.resize(dim_s, false); + for (unsigned int i = 0; i < dim_s; i++) { + e[i] = s[i] - s_star[i]; + } +} + +vpMatrix vpFeatureLuminanceMapping::interaction(unsigned int select) +{ + if (select != FEATURE_ALL) { + throw vpException(vpException::notImplementedError, "cannot compute interaction matrix for a subset of a mapping"); + } + vpMatrix dsdr(dim_s, 6); + interaction(dsdr); + return dsdr; +} +void vpFeatureLuminanceMapping::interaction(vpMatrix &L) +{ + L.resize(dim_s, 6, false, false); + m_featI.interaction(m_LI); + m_mapping->interaction(I, m_LI, s, L); +} + +void vpFeatureLuminanceMapping::print(unsigned int /*select*/) const +{ + std::cout << s << std::endl; +} +END_VISP_NAMESPACE +#endif // C++11 diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp new file mode 100644 index 0000000000..02b1a76d5e --- /dev/null +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -0,0 +1,467 @@ + +/* + * 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: + * Performs various tests on the point class. + */ + +/*! + * \file testLuminanceMapping.cpp + * \example testLuminanceMapping.cpp + */ + +#include + +#include + +#include +#include +#if defined(VISP_HAVE_CATCH2) + +#define CATCH_CONFIG_RUNNER +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +vpMatrix orthogonalBasis(unsigned n, unsigned seed) +{ + vpUniRand rand(seed); + vpMatrix basis(n, n); + vpColVector norms(n); + + //start with random basis + for (unsigned int row = 0; row < n; ++row) { + double norm = 0.0; + for (unsigned int col = 0; col < n; ++col) { + basis[row][col] = rand.uniform(-1.0, 1.0); + norm += basis[row][col] * basis[row][col]; + } + norm = 1.0 / sqrt(norm); + for (unsigned int col = 0; col < n; ++col) { + basis[row][col] *= norm; + } + + } + // Apply gram schmidt process + norms[0] = basis.getRow(0).sumSquare(); + for (unsigned i = 1; i < n; ++i) { + vpColVector uit = basis.getRow(i).t(); + + for (unsigned j = 0; j < i; ++j) { + vpRowVector vj = basis.getRow(j); + vpRowVector res = vj * ((vj * uit) / (norms[j])); + for (unsigned k = 0; k < n; ++k) { + basis[i][k] -= res[k]; + } + } + norms[i] = basis.getRow(i).sumSquare(); + } + for (unsigned int row = 0; row < n; ++row) { + double norm = sqrt(norms[row]); + + for (unsigned int col = 0; col < n; ++col) { + basis[row][col] /= norm; + } + } + return basis; + +} + +SCENARIO("Using PCA features", "[visual_features]") +{ + GIVEN("A matrix containing simple data") + { + const unsigned h = 16, w = 16; + const unsigned numDataPoints = 4; + const unsigned int dataDim = h * w; + const unsigned int trueComponents = 3; + // Generate numDataPoints vectors in a "dataDim"-dimensional space. + // The data is generated from "trueComponents" vectors, that are orthogonal + const vpMatrix orthoFull = (orthogonalBasis(dataDim, 42) + vpMatrix(dataDim, dataDim, 1.0)) * 127.5; // dataDim x dataDim + const vpMatrix ortho(orthoFull, 0, 0, trueComponents, dataDim); // trueComponents X dataDim + const vpMatrix coefficients(numDataPoints, trueComponents); + vpUniRand rand(17); + for (unsigned int i = 0; i < coefficients.getRows(); ++i) { + double sum = 0.0; + for (unsigned int j = 0; j < coefficients.getCols(); ++j) { + coefficients[i][j] = rand.uniform(0.0, 1.0); + sum += coefficients[i][j] * coefficients[i][j]; + } + const double inv_norm = 1.0 / sqrt(sum); + for (unsigned int j = 0; j < coefficients.getCols(); ++j) { + coefficients[i][j] *= inv_norm; + } + } + + vpMatrix data = coefficients * ortho; + + WHEN("Learning PCA basis with too many components") + { + unsigned int k = data.getCols() + 1; + THEN("An exception is thrown") + { + REQUIRE_THROWS(vpLuminancePCA::learn(data.transpose(), k)); + } + } + WHEN("Learning with more images than pixels") + { + vpMatrix wrongData(20, 50); + THEN("An exception is thrown") + { + REQUIRE_THROWS(vpLuminancePCA::learn(wrongData.transpose(), 32)); + } + } + WHEN("Learning PCA basis") + { + for (unsigned int k = 2; k <= trueComponents; ++k) { + + vpLuminancePCA pca = vpLuminancePCA::learn(data.transpose(), k); + const vpMatrix &basis = *pca.getBasis(); + + THEN("Basis has correct dimensions") + { + REQUIRE(basis.getRows() == k); + REQUIRE(basis.getCols() == dataDim); + } + THEN("The basis is orthonormal") + { + const vpMatrix Iapprox = basis * basis.t(); + vpMatrix I; + I.eye(basis.getRows()); + bool matrixSame = true; + for (unsigned int row = 0; row < I.getRows(); ++row) { + for (unsigned int col = 0; col < I.getCols(); ++col) { + if (fabs(I[row][col] - Iapprox[row][col]) > 1e-6) { + matrixSame = false; + break; + } + } + } + REQUIRE(matrixSame); + } + THEN("Mean vector has correct dimensions") + { + REQUIRE(pca.getMean()->getRows() == dataDim); + REQUIRE(pca.getMean()->getCols() == 1); + } + THEN("Modifying the basis size (number of inputs) by hand and saving") + { + const std::string tempDir = vpIoTools::makeTempDirectory("visp_test_pca_wrong"); + const std::string basisFile = vpIoTools::createFilePath(tempDir, "basis.txt"); + const std::string meanFile = vpIoTools::createFilePath(tempDir, "mean.txt"); + const std::string varFile = vpIoTools::createFilePath(tempDir, "var.txt"); + pca.getBasis()->resize(pca.getBasis()->getRows(), pca.getBasis()->getCols() - 1); + REQUIRE_THROWS(pca.save(basisFile, meanFile, varFile)); + } + THEN("Modifying the mean Columns by hand") + { + const std::string tempDir = vpIoTools::makeTempDirectory("visp_test_pca_wrong"); + const std::string basisFile = vpIoTools::createFilePath(tempDir, "basis.txt"); + const std::string meanFile = vpIoTools::createFilePath(tempDir, "mean.txt"); + const std::string varFile = vpIoTools::createFilePath(tempDir, "var.txt"); + + std::shared_ptr mean = pca.getMean(); + mean->resize(mean->getRows() + 1, false); + REQUIRE_THROWS(pca.save(basisFile, meanFile, varFile)); + + } + THEN("Saving and loading pca leads to same basis and mean") + { + const std::string tempDir = vpIoTools::makeTempDirectory("visp_test_pca"); + const std::string basisFile = vpIoTools::createFilePath(tempDir, "basis.txt"); + const std::string meanFile = vpIoTools::createFilePath(tempDir, "mean.txt"); + const std::string varFile = vpIoTools::createFilePath(tempDir, "var.txt"); + + pca.save(basisFile, meanFile, varFile); + + const vpLuminancePCA pca2 = vpLuminancePCA::load(basisFile, meanFile, varFile); + const vpMatrix basisDiff = *pca.getBasis() - *pca2.getBasis(); + const vpColVector meanDiff = *pca.getMean() - *pca2.getMean(); + const vpColVector explainedVarDiff = pca.getExplainedVariance() - pca2.getExplainedVariance(); + bool basisSame = true; + bool meanSame = true; + bool explainedVarSame = true; + + for (unsigned int i = 0; i < basisDiff.getRows(); ++i) { + for (unsigned int j = 0; j < basisDiff.getCols(); ++j) { + if (fabs(basisDiff[i][j]) > 1e-10) { + basisSame = false; + break; + } + } + } + REQUIRE(basisSame); + + for (unsigned int i = 0; i < meanDiff.getRows(); ++i) { + if (fabs(meanDiff[i]) > 1e-10) { + std::cout << meanDiff << std::endl; + meanSame = false; + break; + } + } + REQUIRE(meanSame); + + for (unsigned int i = 0; i < explainedVarDiff.getRows(); ++i) { + if (fabs(explainedVarDiff[i]) > 1e-10) { + explainedVarSame = false; + break; + } + } + REQUIRE(explainedVarSame); + } + + THEN("Explained variance is below 1 and sorted in descending order") + { + const vpColVector var = pca.getExplainedVariance(); + REQUIRE(var.sum() < 1.0); + for (int i = 1; i < (int)var.getRows() - 1; ++i) { + REQUIRE(var[i] >= var[i + 1]); + } + } + if (k == trueComponents) { + WHEN("K is the true manifold dimensionality") + { + THEN("explained variance is close to 1") + { + REQUIRE(pca.getExplainedVariance().sum() > 0.99); + } + THEN("Inverse mapping leads back to the same data") + { + for (unsigned int i = 0; i < numDataPoints; ++i) { + vpImage I(h, w); + for (unsigned int j = 0; j < data.getCols(); ++j) { + I.bitmap[j] = static_cast(data[i][j]); + } + vpColVector s; + pca.setBorder(0); + pca.map(I, s); + vpImage Irec; + pca.inverse(s, Irec); + for (unsigned int j = 0; j < data.getCols(); ++j) { + REQUIRE(abs(static_cast(I.bitmap[j]) - static_cast(Irec.bitmap[j])) < 2); + } + } + } + } + } + + THEN("Projecting data is correct") + { + { + vpColVector s; + pca.setBorder(0); + vpImage I(h, w); + pca.map(I, s); + REQUIRE(s.size() == pca.getProjectionSize()); + } + { + vpColVector s; + const unsigned border = 3; + pca.setBorder(border); + REQUIRE(pca.getBorder() == border); + vpImage I(h + 2 * border, w + 2 * border); + pca.map(I, s); + REQUIRE(s.size() == pca.getProjectionSize()); + } + } + } + } + } + WHEN("Saving unintialized PCA") + { + vpLuminancePCA pca; + const std::string tempDir = vpIoTools::makeTempDirectory("visp_test_pca"); + const std::string basisFile = vpIoTools::createFilePath(tempDir, "basis.txt"); + const std::string meanFile = vpIoTools::createFilePath(tempDir, "mean.txt"); + const std::string varFile = vpIoTools::createFilePath(tempDir, "var.txt"); + THEN("an exception is thrown") + { + REQUIRE_THROWS(pca.save(basisFile, meanFile, varFile)); + } + } +} + +SCENARIO("Using DCT features", "[visual_features]") +{ + + GIVEN("A matrix") + { + std::vector> data = { + { + vpMatrix({ + {0.0, 1.0, 2.0}, + {3.0, 4.0, 5.0}, + {6.0, 7.0, 8.0} + }), + vpColVector( + { 0.0, 1.0, 3.0, 6.0, 4.0, 2.0, 5.0, 7.0, 8.0 } + ), + vpMatrix({ + {0.0, 1.0, 5.0}, + {2.0, 4.0, 6.0}, + {3.0, 7.0, 8.0} + }) + } + }; + for (unsigned int i = 0; i < data.size(); ++i) { + WHEN("Building the associated zigzag indexing matrix") + { + vpMatrix m = std::get<0>(data[i]); + vpColVector contentAsZigzag = std::get<1>(data[i]); + const vpMatrix mAfterWriterVec = std::get<2>(data[i]); + vpLuminanceDCT::vpMatrixZigZagIndex zigzag; + zigzag.init(m.getRows(), m.getCols()); + vpColVector s; + THEN("Calling getValues with wrong matrix rows throws") + { + vpMatrix wrongM(m.getRows() + 1, m.getCols()); + REQUIRE_THROWS(zigzag.getValues(wrongM, 0, 2, s)); + } + THEN("Calling getValues with wrong matrix cols throws") + { + vpMatrix wrongM(m.getRows(), m.getCols() + 1); + REQUIRE_THROWS(zigzag.getValues(wrongM, 0, 2, s)); + } + THEN("Calling getValues with wrong start and end arguments throws") + { + REQUIRE_THROWS(zigzag.getValues(m, 2, 1, s)); + } + THEN("Calling getValues and querying all values returns correct result") + { + REQUIRE_NOTHROW(zigzag.getValues(m, 0, m.size(), s)); + REQUIRE(s == contentAsZigzag); + } + THEN("Calling getValues and querying a subset of the values is correct") + { + REQUIRE_NOTHROW(zigzag.getValues(m, 0, m.size() / 2, s)); + REQUIRE(s == contentAsZigzag.extract(0, m.size() / 2)); + REQUIRE_NOTHROW(zigzag.getValues(m, m.size() / 2, m.size(), s)); + REQUIRE(s == contentAsZigzag.extract(m.size() / 2, m.size() - m.size() / 2)); + } + THEN("Calling setValues with wrong matrix rows throws") + { + vpMatrix wrongM(m.getRows() + 1, m.getCols()); + REQUIRE_THROWS(zigzag.setValues(contentAsZigzag, 0, wrongM)); + } + THEN("Calling setValues with wrong matrix cols throws") + { + vpMatrix wrongM(m.getRows(), m.getCols() + 1); + REQUIRE_THROWS(zigzag.setValues(contentAsZigzag, 0, wrongM)); + } + + THEN("Calling setValues with wrong start and vector size arguments throws") + { + REQUIRE_THROWS(zigzag.setValues(contentAsZigzag, m.size() - contentAsZigzag.size() + 1, m)); + } + + THEN("Calling setValues leads to expected result") + { + vpMatrix mWrite(m.getRows(), m.getCols()); + vpColVector powered = contentAsZigzag; + for (unsigned i = 0; i < powered.size(); ++i) { + powered[i] *= powered[i]; + } + vpColVector poweredRead; + REQUIRE_NOTHROW(zigzag.setValues(powered, 0, mWrite)); + REQUIRE_NOTHROW(zigzag.getValues(mWrite, 0, mWrite.size(), poweredRead)); + REQUIRE(powered == poweredRead); + + vpColVector indices = contentAsZigzag; + for (unsigned i = 0; i < powered.size(); ++i) { + indices[i] = static_cast(i); + } + vpColVector indicesRead; + REQUIRE_NOTHROW(zigzag.setValues(indices, 0, mWrite)); + REQUIRE(mWrite == mAfterWriterVec); + + vpMatrix m2(m.getRows(), m.getCols(), 0.0); + zigzag.setValues(contentAsZigzag.extract(0, 3), 0, m2); + + vpColVector s2; + zigzag.getValues(m2, 0, 3, s2); + REQUIRE(s2 == contentAsZigzag.extract(0, 3)); + + } + + } + } + + + GIVEN("A constant image") + { + vpImage I(32, 64, 20); + WHEN("Computing DCT") + { + vpLuminanceDCT dct(32); + dct.setBorder(0); + vpColVector s; + dct.map(I, s); + THEN("resulting feature vector has correct size") + { + REQUIRE(s.size() == 32); + } + THEN("The only non zero component is the first") + { + REQUIRE(s.sum() == Approx(s[0]).margin(1e-5)); + } + + vpImage Ir; + dct.inverse(s, Ir); + REQUIRE((Ir.getRows() == I.getRows() && Ir.getCols() == I.getCols())); + for (unsigned i = 0; i < I.getRows(); ++i) { + for (unsigned j = 0; j < I.getCols(); ++j) { + const int diff = abs(static_cast(I[i][j]) - static_cast(Ir[i][j])); + REQUIRE(diff < 2); + INFO("i = " + std::to_string(i) + ", j = " + std::to_string(j)); + } + } + } + } + } +} + +int main(int argc, char *argv[]) +{ + Catch::Session session; // There must be exactly one instance + session.applyCommandLine(argc, argv); + + int numFailed = session.run(); + return numFailed; +} + +#else + +int main() +{ + return EXIT_SUCCESS; +} +#endif diff --git a/modules/visual_features/test/feature/testPoint.cpp b/modules/visual_features/test/feature/testPoint.cpp index 2ce5a46ff3..171ddba6ba 100644 --- a/modules/visual_features/test/feature/testPoint.cpp +++ b/modules/visual_features/test/feature/testPoint.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,13 +29,13 @@ * * Description: * Performs various tests on the point class. - * -*****************************************************************************/ + */ /*! - \file testPoint.cpp - \brief Performs various tests on the the point class. -*/ + * \file testPoint.cpp + * \example testPoint.cpp + * \brief Performs various tests on the the point class. + */ #include #include