From 7b5888f74b635e89e56bf29763dea74d88d76a69 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 15 Dec 2023 17:20:20 +0100 Subject: [PATCH 01/28] Structuring vpFeatureLuminancePCA class --- .../visual_features/vpFeatureLuminance.h | 2 +- .../visual_features/vpFeatureLuminancePCA.h | 118 +++++++++++++ .../src/visual-feature/vpFeatureLuminance.cpp | 2 +- .../visual-feature/vpFeatureLuminancePCA.cpp | 156 ++++++++++++++++++ 4 files changed, 276 insertions(+), 2 deletions(-) create mode 100644 modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h create mode 100644 modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h index 5fc3e56f5b..42a7ee1830 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h @@ -117,7 +117,7 @@ class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature void print(unsigned int select = FEATURE_ALL) const override; - void setCameraParameters(vpCameraParameters &_cam); + void setCameraParameters(const vpCameraParameters &_cam); void set_Z(double Z); public: diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h new file mode 100644 index 0000000000..fbc62b318b --- /dev/null +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h @@ -0,0 +1,118 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 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 vpFeatureLuminancePCA_h +#define vpFeatureLuminancePCA_h + +#include + +#include +#include +#include +#include + + +/** + * @brief + * + */ +class VISP_EXPORT vpLuminancePCA +{ +public: + vpLuminancePCA() = default; + vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean); + + unsigned int getProjectionSize() const { return m_basis->getRows(); } + std::shared_ptr getBasis() const { return m_basis; } + std::shared_ptr getMean() const { return m_mean; } + void save(const std::string &basisFilename, const std::string &meanFileName) const; + + + static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName); + + static vpLuminancePCA learn(std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder); + +private: + std::shared_ptr m_basis; + std::shared_ptr m_mean; +}; + + +class VISP_EXPORT vpFeatureLuminancePCA : public vpBasicFeature +{ + +public: + + vpFeatureLuminancePCA(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const vpLuminancePCA &pca); + vpFeatureLuminancePCA(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca); + void init() override; + void init(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const vpLuminancePCA &pca); + void init(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca); + + vpFeatureLuminancePCA(const vpFeatureLuminancePCA &f); + vpFeatureLuminancePCA &operator=(const vpFeatureLuminancePCA &f); + vpFeatureLuminancePCA *duplicate() const override; + + virtual ~vpFeatureLuminancePCA() = default; + + void buildFrom(vpImage &I); + + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, + unsigned int thickness = 1) const override; + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, + unsigned int thickness = 1) const override; + + + vpColVector error(const vpBasicFeature &s_star, unsigned int select = FEATURE_ALL) override; + void error(const vpBasicFeature &s_star, vpColVector &e); + + vpMatrix interaction(unsigned int select = FEATURE_ALL) override; + void interaction(vpMatrix &L); + + + void print(unsigned int select = FEATURE_ALL) const override; + + vpFeatureLuminance &getLuminanceFeature() { return m_featI; } + + + + +private: + + vpLuminancePCA m_pca; + vpFeatureLuminance m_featI; + vpMatrix m_LI; //! Photometric interaction matrix + +}; + +#endif diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 7c48ad5000..2688ff8985 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -161,7 +161,7 @@ void vpFeatureLuminance::set_Z(double Z_) */ double vpFeatureLuminance::get_Z() const { return Z; } -void vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam) { cam = _cam; } +void vpFeatureLuminance::setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; } /*! diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp new file mode 100644 index 0000000000..941969b846 --- /dev/null +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp @@ -0,0 +1,156 @@ +#include + +vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) +{ + +} + +vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std::string &meanFilename) +{ + std::shared_ptr basis = std::make_shared(); + std::shared_ptr mean = std::make_shared(); + vpMatrix::loadMatrix(basisFilename, *basis, true); + vpColVector::load(meanFilename, *mean); + + if (mean->getCols() > 1) { + throw vpException(vpException::badValue, + "Read something that was not a column vector when trying to load the PCA mean vector"); + } + 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); +} + +void vpLuminancePCA::save(const std::string &basisFilename, const std::string &meanFilename) 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); + vpColVector::save(meanFilename, *m_mean, true); +} + +static vpLuminancePCA learn(std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder) +{ + +} + +vpFeatureLuminancePCA::vpFeatureLuminancePCA(const vpCameraParameters &cam, +unsigned int h, unsigned int w, double Z, const vpLuminancePCA &pca) +{ + init(cam, h, w, Z, pca); +} + +vpFeatureLuminancePCA::vpFeatureLuminancePCA(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca) +{ + init(luminance, pca); +} +vpFeatureLuminancePCA::vpFeatureLuminancePCA(const vpFeatureLuminancePCA &f) +{ + *this = f; +} + +void vpFeatureLuminancePCA::init() +{ + dim_s = 0; + m_featI.init(0, 0, 0.0); +} + +void vpFeatureLuminancePCA::init( + const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, + const vpLuminancePCA &pca) +{ + m_featI.init(h, w, Z); + m_featI.setCameraParameters(cam); + m_pca = pca; + dim_s = pca.getProjectionSize(); + s.resize(dim_s, true); +} +void vpFeatureLuminancePCA::init(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca) +{ + m_featI = luminance; + m_pca = pca; + dim_s = pca.getProjectionSize(); + s.resize(dim_s, true); +} + + +vpFeatureLuminancePCA &vpFeatureLuminancePCA::operator=(const vpFeatureLuminancePCA &f) +{ + dim_s = f.dim_s; + s = f.s; + m_pca = f.m_pca; + m_featI = f.m_featI; +} +vpFeatureLuminancePCA *vpFeatureLuminancePCA::duplicate() const +{ + return new vpFeatureLuminancePCA(*this); +} + + +void vpFeatureLuminancePCA::buildFrom(vpImage &I) { } + +void vpFeatureLuminancePCA::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, + unsigned int thickness) const +{ } +void vpFeatureLuminancePCA::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, + unsigned int thickness) const +{ } + + +vpColVector vpFeatureLuminancePCA::error(const vpBasicFeature &s_star, unsigned int select) +{ + if (select != FEATURE_ALL) { + throw vpException(vpException::notImplementedError, "cannot compute error on subset of PCA features"); + } + vpColVector e(dim_s); + error(s_star, e); + return e; +} + +void vpFeatureLuminancePCA::error(const vpBasicFeature &s_star, vpColVector &e) +{ + // if (dim_s != s_star.dimension_s()) { + // throw vpException(vpException::dimensionError, "Wrong dimensions when computing error in PCA features"); + // } + e.resize(dim_s, false); + for (unsigned int i = 0; i < dim_s; i++) { + e[i] = s[i] - s_star[i]; + } +} + +vpMatrix vpFeatureLuminancePCA::interaction(unsigned int select) +{ + if (select != FEATURE_ALL) { + throw vpException(vpException::notImplementedError, "cannot compute interaction matrix for a subset of PCA features"); + } + vpMatrix dWdr(dim_s, 6); + interaction(dWdr); + return dWdr; +} +void vpFeatureLuminancePCA::interaction(vpMatrix &L) +{ + L.resize(dim_s, 6, false, false); + m_featI.interaction(m_LI); +} + + +void vpFeatureLuminancePCA::print(unsigned int select) const +{ + if (select != FEATURE_ALL) { + throw vpException(vpException::notImplementedError, "cannot print subset of PCA features"); + } + +} From dd43fea3e1310e8a859b31535d8182974eac9ae6 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Mon, 8 Jan 2024 13:05:11 +0100 Subject: [PATCH 02/28] work done during holidays --- .../visual_features/vpFeatureLuminancePCA.h | 40 +++++++++++++------ 1 file changed, 27 insertions(+), 13 deletions(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h index fbc62b318b..c87aae804c 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h @@ -31,8 +31,8 @@ * Luminance based feature. */ -#ifndef vpFeatureLuminancePCA_h -#define vpFeatureLuminancePCA_h +#ifndef vpFeatureLuminanceMapping_h +#define vpFeatureLuminanceMapping_h #include @@ -42,6 +42,19 @@ #include +class VISP_EXPORT vpLuminanceMapping +{ + + void map(const vpImage &I, vpColVector &s); + void inverse(const vpColVector &s, vpImage &I); + + unsigned int getProjectionSize() const { return m_basis->getRows(); } + +private: + const unsigned m_mappingSize; +}; + + /** * @brief * @@ -67,23 +80,23 @@ class VISP_EXPORT vpLuminancePCA std::shared_ptr m_mean; }; - -class VISP_EXPORT vpFeatureLuminancePCA : public vpBasicFeature +template +class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature { public: - vpFeatureLuminancePCA(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const vpLuminancePCA &pca); - vpFeatureLuminancePCA(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca); + vpFeatureLuminanceMapping(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const Mapping &mapping); + vpFeatureLuminanceMapping(const vpFeatureLuminance &luminance, const Mapping &pca); void init() override; - void init(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const vpLuminancePCA &pca); - void init(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca); + void init(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const Mapping &mapping); + void init(const vpFeatureLuminance &luminance, const Mapping &mapping); - vpFeatureLuminancePCA(const vpFeatureLuminancePCA &f); - vpFeatureLuminancePCA &operator=(const vpFeatureLuminancePCA &f); - vpFeatureLuminancePCA *duplicate() const override; + vpFeatureLuminanceMapping(const vpFeatureLuminanceMapping &f); + vpFeatureLuminanceMapping &operator=(const vpFeatureLuminanceMapping &f); + vpFeatureLuminanceMapping *duplicate() const override; - virtual ~vpFeatureLuminancePCA() = default; + virtual ~vpFeatureLuminanceMapping() = default; void buildFrom(vpImage &I); @@ -103,13 +116,14 @@ class VISP_EXPORT vpFeatureLuminancePCA : public vpBasicFeature void print(unsigned int select = FEATURE_ALL) const override; vpFeatureLuminance &getLuminanceFeature() { return m_featI; } + Mapping &getMapping() { return m_mapping; } private: - vpLuminancePCA m_pca; + Mapping m_mapping; vpFeatureLuminance m_featI; vpMatrix m_LI; //! Photometric interaction matrix From e425ab1ae5809d60fa59a0c3638c25eafae10f45 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 9 Jan 2024 13:35:41 +0100 Subject: [PATCH 03/28] rework classes, remove uneeded templating --- ...nancePCA.h => vpFeatureLuminanceMapping.h} | 33 ++++----- ...ePCA.cpp => vpFeatureLuminanceMapping.cpp} | 68 +++++++++++-------- 2 files changed, 57 insertions(+), 44 deletions(-) rename modules/visual_features/include/visp3/visual_features/{vpFeatureLuminancePCA.h => vpFeatureLuminanceMapping.h} (75%) rename modules/visual_features/src/visual-feature/{vpFeatureLuminancePCA.cpp => vpFeatureLuminanceMapping.cpp} (58%) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h similarity index 75% rename from modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h rename to modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index c87aae804c..d81099e713 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminancePCA.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -44,11 +44,13 @@ class VISP_EXPORT vpLuminanceMapping { +public: + vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } + virtual void map(const vpImage &I, vpColVector &s) = 0; + virtual void interaction(const vpImage &I, const vpColVector &s, vpMatrix &L) = 0; + virtual void inverse(const vpColVector &s, vpImage &I) = 0; - void map(const vpImage &I, vpColVector &s); - void inverse(const vpColVector &s, vpImage &I); - - unsigned int getProjectionSize() const { return m_basis->getRows(); } + unsigned int getProjectionSize() const { return m_mappingSize; } private: const unsigned m_mappingSize; @@ -59,20 +61,20 @@ class VISP_EXPORT vpLuminanceMapping * @brief * */ -class VISP_EXPORT vpLuminancePCA +class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping { public: - vpLuminancePCA() = default; + vpLuminancePCA() : vpLuminanceMapping(0) { } vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean); unsigned int getProjectionSize() const { return m_basis->getRows(); } std::shared_ptr getBasis() const { return m_basis; } std::shared_ptr getMean() const { return m_mean; } + void map(const vpImage &I, vpColVector &s) override; + void inverse(const vpColVector &s, vpImage &I) override; + void interaction(const vpImage &I, const vpColVector &s, vpMatrix &L) override; void save(const std::string &basisFilename, const std::string &meanFileName) const; - - static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName); - static vpLuminancePCA learn(std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder); private: @@ -80,17 +82,16 @@ class VISP_EXPORT vpLuminancePCA std::shared_ptr m_mean; }; -template class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature { public: - vpFeatureLuminanceMapping(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const Mapping &mapping); - vpFeatureLuminanceMapping(const vpFeatureLuminance &luminance, const Mapping &pca); + 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() override; - void init(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, const Mapping &mapping); - void init(const vpFeatureLuminance &luminance, const Mapping &mapping); + 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); @@ -116,14 +117,14 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature void print(unsigned int select = FEATURE_ALL) const override; vpFeatureLuminance &getLuminanceFeature() { return m_featI; } - Mapping &getMapping() { return m_mapping; } + std::shared_ptr &getMapping() { return m_mapping; } private: - Mapping m_mapping; + std::shared_ptr m_mapping; vpFeatureLuminance m_featI; vpMatrix m_LI; //! Photometric interaction matrix diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp similarity index 58% rename from modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp rename to modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 941969b846..570cfe41b8 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminancePCA.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -1,6 +1,17 @@ -#include +#include -vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) +vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) : vpLuminanceMapping(basis->getRows()) +{ + if (basis->getRows() != mean->getRows()) { + throw vpException(vpException::dimensionError, "PCA mean and basis should have the same number of components"); + } +} + +void vpLuminancePCA::map(const vpImage &I, vpColVector &s) +{ + +} +void vpLuminancePCA::inverse(const vpColVector &s, vpImage &I) { } @@ -47,70 +58,71 @@ static vpLuminancePCA learn(std::vector> &images, const u } -vpFeatureLuminancePCA::vpFeatureLuminancePCA(const vpCameraParameters &cam, -unsigned int h, unsigned int w, double Z, const vpLuminancePCA &pca) +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpCameraParameters &cam, +unsigned int h, unsigned int w, double Z, std::shared_ptr mapping) { - init(cam, h, w, Z, pca); + init(cam, h, w, Z, mapping); } -vpFeatureLuminancePCA::vpFeatureLuminancePCA(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca) +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminance &luminance, std::shared_ptr mapping) { - init(luminance, pca); + init(luminance, mapping); } -vpFeatureLuminancePCA::vpFeatureLuminancePCA(const vpFeatureLuminancePCA &f) +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminanceMapping &f) { *this = f; } -void vpFeatureLuminancePCA::init() +void vpFeatureLuminanceMapping::init() { dim_s = 0; m_featI.init(0, 0, 0.0); } -void vpFeatureLuminancePCA::init( +void vpFeatureLuminanceMapping::init( const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, - const vpLuminancePCA &pca) + std::shared_ptr mapping) { m_featI.init(h, w, Z); m_featI.setCameraParameters(cam); - m_pca = pca; - dim_s = pca.getProjectionSize(); + m_mapping = mapping; + dim_s = m_mapping->getProjectionSize(); s.resize(dim_s, true); } -void vpFeatureLuminancePCA::init(const vpFeatureLuminance &luminance, const vpLuminancePCA &pca) +void vpFeatureLuminanceMapping::init(const vpFeatureLuminance &luminance, std::shared_ptr mapping) { m_featI = luminance; - m_pca = pca; - dim_s = pca.getProjectionSize(); + m_mapping = mapping; + dim_s = m_mapping->getProjectionSize(); s.resize(dim_s, true); } -vpFeatureLuminancePCA &vpFeatureLuminancePCA::operator=(const vpFeatureLuminancePCA &f) +vpFeatureLuminanceMapping &vpFeatureLuminanceMapping::operator=(const vpFeatureLuminanceMapping &f) { dim_s = f.dim_s; s = f.s; - m_pca = f.m_pca; + m_mapping = f.m_mapping; m_featI = f.m_featI; + return *this; } -vpFeatureLuminancePCA *vpFeatureLuminancePCA::duplicate() const +vpFeatureLuminanceMapping *vpFeatureLuminanceMapping::duplicate() const { - return new vpFeatureLuminancePCA(*this); + return new vpFeatureLuminanceMapping(*this); } -void vpFeatureLuminancePCA::buildFrom(vpImage &I) { } +void vpFeatureLuminanceMapping::buildFrom(vpImage &I) { } -void vpFeatureLuminancePCA::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, +void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { } -void vpFeatureLuminancePCA::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, +void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { } -vpColVector vpFeatureLuminancePCA::error(const vpBasicFeature &s_star, unsigned int select) +vpColVector vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, unsigned int select) { if (select != FEATURE_ALL) { throw vpException(vpException::notImplementedError, "cannot compute error on subset of PCA features"); @@ -120,7 +132,7 @@ vpColVector vpFeatureLuminancePCA::error(const vpBasicFeature &s_star, unsigned return e; } -void vpFeatureLuminancePCA::error(const vpBasicFeature &s_star, vpColVector &e) +void vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, vpColVector &e) { // if (dim_s != s_star.dimension_s()) { // throw vpException(vpException::dimensionError, "Wrong dimensions when computing error in PCA features"); @@ -131,7 +143,7 @@ void vpFeatureLuminancePCA::error(const vpBasicFeature &s_star, vpColVector &e) } } -vpMatrix vpFeatureLuminancePCA::interaction(unsigned int select) +vpMatrix vpFeatureLuminanceMapping::interaction(unsigned int select) { if (select != FEATURE_ALL) { throw vpException(vpException::notImplementedError, "cannot compute interaction matrix for a subset of PCA features"); @@ -140,14 +152,14 @@ vpMatrix vpFeatureLuminancePCA::interaction(unsigned int select) interaction(dWdr); return dWdr; } -void vpFeatureLuminancePCA::interaction(vpMatrix &L) +void vpFeatureLuminanceMapping::interaction(vpMatrix &L) { L.resize(dim_s, 6, false, false); m_featI.interaction(m_LI); } -void vpFeatureLuminancePCA::print(unsigned int select) const +void vpFeatureLuminanceMapping::print(unsigned int select) const { if (select != FEATURE_ALL) { throw vpException(vpException::notImplementedError, "cannot print subset of PCA features"); From 8d9a58ebd58a84822c6d2a12402f459ad04edd89 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 13 Feb 2024 02:06:12 +0100 Subject: [PATCH 04/28] learn PCA, needs testing --- .../vpFeatureLuminanceMapping.h | 12 ++- .../vpFeatureLuminanceMapping.cpp | 88 ++++++++++++++++++- .../test/feature/testLuminanceMapping.cpp | 0 3 files changed, 92 insertions(+), 8 deletions(-) create mode 100644 modules/visual_features/test/feature/testLuminanceMapping.cpp diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index d81099e713..245c9b1c3b 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -68,20 +68,24 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean); unsigned int getProjectionSize() const { return m_basis->getRows(); } - std::shared_ptr getBasis() const { return m_basis; } - std::shared_ptr getMean() const { return m_mean; } + vpMatrix getBasis() const { return *m_basis; } + vpColVector getMean() const { return *m_mean; } void map(const vpImage &I, vpColVector &s) override; void inverse(const vpColVector &s, vpImage &I) override; void interaction(const vpImage &I, const vpColVector &s, vpMatrix &L) override; + void save(const std::string &basisFilename, const std::string &meanFileName) const; static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName); - static vpLuminancePCA learn(std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder); + + static vpLuminancePCA learn(const std::vector &imageFiles, const unsigned int projectionSize, const unsigned int imageBorder = 0); + static vpLuminancePCA learn(const std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder = 0); + static vpLuminancePCA learn(const vpMatrix &images, const unsigned int projectionSize); + private: std::shared_ptr m_basis; std::shared_ptr m_mean; }; - class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature { diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 570cfe41b8..ae487ed12a 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -1,9 +1,11 @@ #include -vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) : vpLuminanceMapping(basis->getRows()) +#include + +vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) : vpLuminanceMapping(basis->getRows()), m_basis(basis), m_mean(mean) { - if (basis->getRows() != mean->getRows()) { - throw vpException(vpException::dimensionError, "PCA mean and basis should have the same number of components"); + if (basis->getCols() != mean->getRows()) { + throw vpException(vpException::dimensionError, "PCA mean and basis should have the same number of inputs"); } } @@ -16,6 +18,11 @@ void vpLuminancePCA::inverse(const vpColVector &s, vpImage &I) } +void vpLuminancePCA::interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) +{ + L = (*basis) * LI; +} + vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std::string &meanFilename) { std::shared_ptr basis = std::make_shared(); @@ -53,11 +60,84 @@ void vpLuminancePCA::save(const std::string &basisFilename, const std::string &m vpColVector::save(meanFilename, *m_mean, true); } -static vpLuminancePCA learn(std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder) +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); +} +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); } +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)"); + } + vpColVector mean(images.getRows(), 0.0); + for (int i = 0; i < images.getCols(); ++i) { + mean += images.getCol(i); + } + mean /= images.getCols(); + + + vpMatrix centered(images.getRows(), images.getCols()); + for (int i = 0; i < centered.getRows(); ++i) { + for (int j = 0; j < centered.getCols(); ++j) { + centered[i][j] = images[i][j] - mean[i]; + } + } + std::cout << centered << std::endl; + vpColVector eigenValues; + vpMatrix V; + centered.svd(eigenValues, V); + + std::shared_ptr basis = std::make_shared(projectionSize, centered.getRows()); + for (unsigned int i = 0; i < projectionSize; ++i) { + for (unsigned int j = 0; j < centered.getRows(); ++j) { + (*basis)[i][j] = centered[j][i]; + } + } + std::shared_ptr meanPtr = std::make_shared(mean); + std::cout << centered.getRows() << " " << centered.getCols() << std::endl; + return vpLuminancePCA(basis, meanPtr); +} + + vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, std::shared_ptr mapping) { diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp new file mode 100644 index 0000000000..e69de29bb2 From 46c8d28fe6a56e35982c3210c52516371f43642b Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 13 Feb 2024 02:41:20 +0100 Subject: [PATCH 05/28] Learn PCA, start test --- modules/visual_features/CMakeLists.txt | 7 ++++- .../vpFeatureLuminanceMapping.h | 10 +++--- .../vpFeatureLuminanceMapping.cpp | 21 ++++++++----- .../test/feature/testLuminanceMapping.cpp | 31 +++++++++++++++++++ 4 files changed, 55 insertions(+), 14 deletions(-) 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/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 245c9b1c3b..41cc50207f 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -47,7 +47,7 @@ class VISP_EXPORT vpLuminanceMapping public: vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } virtual void map(const vpImage &I, vpColVector &s) = 0; - virtual void interaction(const vpImage &I, const vpColVector &s, vpMatrix &L) = 0; + virtual void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) = 0; virtual void inverse(const vpColVector &s, vpImage &I) = 0; unsigned int getProjectionSize() const { return m_mappingSize; } @@ -66,18 +66,17 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping public: vpLuminancePCA() : vpLuminanceMapping(0) { } vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean); - - unsigned int getProjectionSize() const { return m_basis->getRows(); } vpMatrix getBasis() const { return *m_basis; } vpColVector getMean() const { return *m_mean; } void map(const vpImage &I, vpColVector &s) override; void inverse(const vpColVector &s, vpImage &I) override; - void interaction(const vpImage &I, const vpColVector &s, vpMatrix &L) override; + void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) override; void save(const std::string &basisFilename, const std::string &meanFileName) const; static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName); - +#ifdef VISP_HAVE_MODULE_IO static vpLuminancePCA learn(const std::vector &imageFiles, const unsigned int projectionSize, const unsigned int imageBorder = 0); +#endif static vpLuminancePCA learn(const std::vector> &images, const unsigned int projectionSize, const unsigned int imageBorder = 0); static vpLuminancePCA learn(const vpMatrix &images, const unsigned int projectionSize); @@ -86,6 +85,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping std::shared_ptr m_basis; std::shared_ptr m_mean; }; + class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature { diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index ae487ed12a..17960b0c8c 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -1,6 +1,8 @@ #include +#ifdef VISP_HAVE_MODULE_IO #include +#endif vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) : vpLuminanceMapping(basis->getRows()), m_basis(basis), m_mean(mean) { @@ -20,7 +22,7 @@ void vpLuminancePCA::inverse(const vpColVector &s, vpImage &I) void vpLuminancePCA::interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) { - L = (*basis) * LI; + L = (*m_basis) * LI; } vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std::string &meanFilename) @@ -82,6 +84,7 @@ vpLuminancePCA vpLuminancePCA::learn(const std::vector> & 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; @@ -102,33 +105,35 @@ vpLuminancePCA vpLuminancePCA::learn(const std::vector &imageFiles, } 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)"); } + // Mean computation vpColVector mean(images.getRows(), 0.0); - for (int i = 0; i < images.getCols(); ++i) { + 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 (int i = 0; i < centered.getRows(); ++i) { - for (int j = 0; j < centered.getCols(); ++j) { + for (unsigned i = 0; i < centered.getRows(); ++i) { + for (unsigned j = 0; j < centered.getCols(); ++j) { centered[i][j] = images[i][j] - mean[i]; } } - std::cout << centered << std::endl; + vpColVector eigenValues; vpMatrix V; centered.svd(eigenValues, V); std::shared_ptr basis = std::make_shared(projectionSize, centered.getRows()); - for (unsigned int i = 0; i < projectionSize; ++i) { - for (unsigned int j = 0; j < centered.getRows(); ++j) { + for (unsigned i = 0; i < projectionSize; ++i) { + for (unsigned j = 0; j < centered.getRows(); ++j) { (*basis)[i][j] = centered[j][i]; } } diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index e69de29bb2..0f4978a4a5 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -0,0 +1,31 @@ + +#include + +#if defined(VISP_HAVE_CATCH2) + +#define CATCH_CONFIG_RUNNER +#include + + + + +SCENARIO("Learning a new basis for PCA", "[visual_features]") +{ + vpMatrix data(50, 100); + vpLuminancePCA::learn(data, 32); +} +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; +} From 6f4aef0db8d9284996ebe092443dc0c9f7578cdf Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 14 Feb 2024 18:20:04 +0100 Subject: [PATCH 06/28] working on luminance --- .../visual_features/src/visual-feature/vpFeatureLuminance.cpp | 3 +++ modules/visual_features/test/feature/testLuminanceMapping.cpp | 1 + 2 files changed, 4 insertions(+) diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 2688ff8985..e60183ff41 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -99,6 +99,9 @@ vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixIn { nbParameters = 1; dim_s = 0; + if (flags != nullptr) { + delete[] flags; + } flags = nullptr; init(); diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 0f4978a4a5..97df252a5b 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -29,3 +29,4 @@ int main() { return EXIT_SUCCESS; } +#endif From 01b5e48f91da6cbd43167cd303b75ac845ae0481 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 15 Feb 2024 02:12:45 +0100 Subject: [PATCH 07/28] testing learning pca --- .../vpFeatureLuminanceMapping.cpp | 15 ++++--- .../test/feature/testLuminanceMapping.cpp | 43 +++++++++++++++++-- 2 files changed, 49 insertions(+), 9 deletions(-) diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 17960b0c8c..12ed939476 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -109,7 +109,7 @@ vpLuminancePCA vpLuminancePCA::learn(const std::vector &imageFiles, vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int projectionSize) { - if (projectionSize > images.getRows() && projectionSize > images.getCols()) { + 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)"); } // Mean computation @@ -130,15 +130,18 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int vpColVector eigenValues; vpMatrix V; centered.svd(eigenValues, V); - - std::shared_ptr basis = std::make_shared(projectionSize, centered.getRows()); - for (unsigned i = 0; i < projectionSize; ++i) { - for (unsigned j = 0; j < centered.getRows(); ++j) { - (*basis)[i][j] = centered[j][i]; + std::cout << eigenValues << std::endl; + vpMatrix U(centered.getRows(), projectionSize); + std::cout << "CENTERED : " << centered << std::endl; + for (unsigned i = 0; i < centered.getRows(); ++i) { + for (unsigned j = 0; j < projectionSize; ++j) { + U[i][j] = centered[i][j]; } } + std::shared_ptr basis = std::make_shared(U.t()); std::shared_ptr meanPtr = std::make_shared(mean); std::cout << centered.getRows() << " " << centered.getCols() << std::endl; + std::cout << "basis: " << *basis << std::endl; return vpLuminancePCA(basis, meanPtr); } diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 97df252a5b..f6841a7b9c 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -9,10 +9,47 @@ -SCENARIO("Learning a new basis for PCA", "[visual_features]") +SCENARIO("Using PCA features", "[visual_features]") { - vpMatrix data(50, 100); - vpLuminancePCA::learn(data, 32); + GIVEN("A matrix containing simple data") + { + unsigned int dataCols = 10; + unsigned int trueComponents = dataCols / 2; + vpMatrix data(40, dataCols, 0.0); + std::cout << data.size() << std::endl; + std::cout << data.getRows() << " " << data.getCols() << std::endl; + for (unsigned c = 0; c < dataCols / 2; ++c) { + unsigned start = c * data.getRows() / trueComponents; + unsigned end = (c + 1) * (data.getRows() / trueComponents); + std::cout << start << " " << end << std::endl; + for (unsigned row = start; row < end; ++row) { + data[row][c] = 1; + } + } + + std::cout << data << std::endl; + + 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 PCA basis") + { + unsigned int k = dataCols / 2; + vpLuminancePCA pca = vpLuminancePCA::learn(data.transpose(), k); + vpMatrix basis = pca.getBasis(); + THEN("Basis has correct dimensions") + { + REQUIRE(basis.getRows() == k); + REQUIRE(basis.getCols() == dataCols); + } + } + + } } int main(int argc, char *argv[]) { From 8c0e2de593d78ea2a5b803c7f9b1dc53dda5cc07 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Sat, 17 Feb 2024 03:06:18 +0100 Subject: [PATCH 08/28] More testing of pca, explained variance --- doc/biblio/references.bib | 27 ++- .../vpFeatureLuminanceMapping.h | 134 +++++++++++--- .../vpFeatureLuminanceMapping.cpp | 49 +++-- .../test/feature/testLuminanceMapping.cpp | 175 ++++++++++++++++-- 4 files changed, 328 insertions(+), 57 deletions(-) diff --git a/doc/biblio/references.bib b/doc/biblio/references.bib index ec1032cbe6..d886d7c890 100644 --- a/doc/biblio/references.bib +++ b/doc/biblio/references.bib @@ -674,9 +674,34 @@ @article{Malvar2004HighqualityLI volume = {3}, pages = {iii-485} } + @inproceedings{Labbe2022Megapose, title = {{{MegaPose}}: {{6D Pose Estimation}} of {{Novel Objects}} via {{Render}} \& {{Compare}}}, booktitle = {CoRL}, author = {Labb\'e, Yann and Manuelli, Lucas and Mousavian, Arsalan and Tyree, Stephen and Birchfield, Stan and Tremblay, Jonathan and Carpentier, Justin and Aubry, Mathieu and Fox, Dieter and Sivic, Josef}, date = {2022} -} \ No newline at end of file +} + +@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} +} diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 41cc50207f..75222c0828 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -41,39 +41,130 @@ #include #include - +/** + * @brief Base class for functions that map an image and its interaction matrix to a different domain. + * * 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 \param I to a representation \param s. + * This representation s has getProjectionSize() rows. + * + * Note that when combined with vpFeatureLuminanceMapping, + * The image \param I does not have the same size as the image input of vpFeatureLuminanceMapping::buildFrom. + * \param 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 \param s + * + * @param I input image used to compute s + * @param LI Photometric interaction matrix associated to \param I (see vpFeatureLuminance) + * @param s the already 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 \param I from a representation \param 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; } -private: - const unsigned m_mappingSize; +protected: + unsigned m_mappingSize; }; /** - * @brief + * @brief Implementation of \cite{Marchand19a}. + * + * 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) { } - vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean); - vpMatrix getBasis() const { return *m_basis; } - vpColVector getMean() const { return *m_mean; } - void map(const vpImage &I, vpColVector &s) override; - void inverse(const vpColVector &s, vpImage &I) override; - void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) override; - - void save(const std::string &basisFilename, const std::string &meanFileName) const; - static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName); + + vpLuminancePCA() : vpLuminanceMapping(0), m_basis(nullptr), m_mean(nullptr) { } + + /** + * @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 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; + + void save(const std::string &basisFilename, const std::string &meanFileName, const std::string &explainedVarianceFile) const; + static vpLuminancePCA load(const std::string &basisFilename, const std::string &meanFileName, const std::string &explainedVarianceFile); #ifdef VISP_HAVE_MODULE_IO static vpLuminancePCA learn(const std::vector &imageFiles, const unsigned int projectionSize, const unsigned int imageBorder = 0); #endif @@ -84,6 +175,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping private: std::shared_ptr m_basis; std::shared_ptr m_mean; + vpColVector m_explainedVariance; }; class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature @@ -93,32 +185,32 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature 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() override; + 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 override; + vpFeatureLuminanceMapping *duplicate() const vp_override; virtual ~vpFeatureLuminanceMapping() = default; void buildFrom(vpImage &I); void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, - unsigned int thickness = 1) const override; + unsigned int thickness = 1) const vp_override; void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, - unsigned int thickness = 1) const override; + unsigned int thickness = 1) const vp_override; - vpColVector error(const vpBasicFeature &s_star, unsigned int select = FEATURE_ALL) 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) override; + vpMatrix interaction(unsigned int select = FEATURE_ALL) vp_override; void interaction(vpMatrix &L); - void print(unsigned int select = FEATURE_ALL) const override; + void print(unsigned int select = FEATURE_ALL) const vp_override; vpFeatureLuminance &getLuminanceFeature() { return m_featI; } std::shared_ptr &getMapping() { return m_mapping; } diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 12ed939476..c3a024b806 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -4,11 +4,24 @@ #include #endif -vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean) : vpLuminanceMapping(basis->getRows()), m_basis(basis), m_mean(mean) +vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean, const vpColVector &explainedVariance) + : vpLuminanceMapping(basis->getRows()) +{ + 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; } void vpLuminancePCA::map(const vpImage &I, vpColVector &s) @@ -25,17 +38,23 @@ void vpLuminancePCA::interaction(const vpImage &I, const vpMatrix L = (*m_basis) * LI; } -vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std::string &meanFilename) +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(); - vpMatrix::loadMatrix(basisFilename, *basis, true); - vpColVector::load(meanFilename, *mean); + vpColVector explainedVariance; + vpMatrix::loadMatrix(basisFilename, *basis, false); + vpMatrix::loadMatrix(meanFilename, *mean, false); + vpMatrix::loadMatrix(explainedVarianceFile, explainedVariance, false); if (mean->getCols() > 1) { - throw vpException(vpException::badValue, + 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"; @@ -45,10 +64,10 @@ vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std: throw vpException(vpException::dimensionError, ss.str()); } - return vpLuminancePCA(basis, mean); + return vpLuminancePCA(basis, mean, explainedVariance); } -void vpLuminancePCA::save(const std::string &basisFilename, const std::string &meanFilename) const +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, @@ -58,8 +77,9 @@ void vpLuminancePCA::save(const std::string &basisFilename, const std::string &m 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); - vpColVector::save(meanFilename, *m_mean, true); + vpMatrix::saveMatrix(basisFilename, *m_basis, false); + vpMatrix::saveMatrix(meanFilename, *m_mean, false); + vpMatrix::saveMatrix(explainedVarianceFile, m_explainedVariance, false); } vpLuminancePCA vpLuminancePCA::learn(const std::vector> &images, const unsigned int projectionSize, const unsigned int border) @@ -129,20 +149,19 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int vpColVector eigenValues; vpMatrix V; - centered.svd(eigenValues, V); - std::cout << eigenValues << std::endl; + centered.svdOpenCV(eigenValues, V); vpMatrix U(centered.getRows(), projectionSize); - std::cout << "CENTERED : " << centered << std::endl; 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); - std::cout << centered.getRows() << " " << centered.getCols() << std::endl; - std::cout << "basis: " << *basis << std::endl; - return vpLuminancePCA(basis, meanPtr); + return vpLuminancePCA(basis, meanPtr, componentsExplainedVar); } diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index f6841a7b9c..953e05adab 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -1,33 +1,79 @@ #include +#include +#include +#include #if defined(VISP_HAVE_CATCH2) #define CATCH_CONFIG_RUNNER #include +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") { - unsigned int dataCols = 10; - unsigned int trueComponents = dataCols / 2; - vpMatrix data(40, dataCols, 0.0); - std::cout << data.size() << std::endl; - std::cout << data.getRows() << " " << data.getCols() << std::endl; - for (unsigned c = 0; c < dataCols / 2; ++c) { - unsigned start = c * data.getRows() / trueComponents; - unsigned end = (c + 1) * (data.getRows() / trueComponents); - std::cout << start << " " << end << std::endl; - for (unsigned row = start; row < end; ++row) { - data[row][c] = 1; + const unsigned numDataPoints = 200; + const unsigned int dataDim = 50; + const unsigned int trueComponents = 5; + // Generate numDataPoints vectors in a "dataDim"-dimensional space. + // The data is generated from "trueComponents" vectors, that are orthogonal + const vpMatrix orthoFull = orthogonalBasis(dataDim, 42); // 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) { + for (unsigned int j = 0; j < coefficients.getCols(); ++j) { + coefficients[i][j] = rand.uniform(-1.0, 1.0); } } - std::cout << data << std::endl; + vpMatrix data = coefficients * ortho; WHEN("Learning PCA basis with too many components") { @@ -39,16 +85,105 @@ SCENARIO("Using PCA features", "[visual_features]") } WHEN("Learning PCA basis") { - unsigned int k = dataCols / 2; - vpLuminancePCA pca = vpLuminancePCA::learn(data.transpose(), k); - vpMatrix basis = pca.getBasis(); - THEN("Basis has correct dimensions") - { - REQUIRE(basis.getRows() == k); - REQUIRE(basis.getCols() == dataCols); + for (unsigned int k = 1; k <= trueComponents; ++k) { + + const 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("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) { + 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("Projecting data has correct dimensions") + { + + } } } - } } int main(int argc, char *argv[]) From a67c81199bcbf315aae7cabcd5dd2bf36aa321f0 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Sat, 17 Feb 2024 16:42:48 +0100 Subject: [PATCH 09/28] commit before merge master --- .../visp3/visual_features/vpFeatureLuminanceMapping.h | 2 +- .../src/visual-feature/vpFeatureLuminanceMapping.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 75222c0828..0be6852d75 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -145,7 +145,6 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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; } @@ -223,6 +222,7 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature std::shared_ptr m_mapping; vpFeatureLuminance m_featI; vpMatrix m_LI; //! Photometric interaction matrix + vpImage I; }; diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index c3a024b806..d81e3ae387 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -4,7 +4,7 @@ #include #endif -vpLuminancePCA::vpLuminancePCA(std::shared_ptr basis, std::shared_ptr mean, const vpColVector &explainedVariance) +vpLuminancePCA::vpLuminancePCA(const std::shared_ptr &basis, const std::shared_ptr &mean, const vpColVector &explainedVariance) : vpLuminanceMapping(basis->getRows()) { init(basis, mean, explainedVariance); @@ -255,14 +255,15 @@ vpMatrix vpFeatureLuminanceMapping::interaction(unsigned int select) if (select != FEATURE_ALL) { throw vpException(vpException::notImplementedError, "cannot compute interaction matrix for a subset of PCA features"); } - vpMatrix dWdr(dim_s, 6); - interaction(dWdr); - return dWdr; + 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); } @@ -271,5 +272,5 @@ void vpFeatureLuminanceMapping::print(unsigned int select) const if (select != FEATURE_ALL) { throw vpException(vpException::notImplementedError, "cannot print subset of PCA features"); } - + std::cout << s << std::endl; } From 3807899bb5a1a0532266ecc246e75ae263af9d2b Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Mon, 19 Feb 2024 13:20:18 +0100 Subject: [PATCH 10/28] rework test case, mapping etc. --- .../vpFeatureLuminanceMapping.h | 75 +++++++++++++++---- .../vpFeatureLuminanceMapping.cpp | 36 ++++++++- 2 files changed, 95 insertions(+), 16 deletions(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 0be6852d75..772613ec75 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -57,27 +57,27 @@ class VISP_EXPORT vpLuminanceMapping */ vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } /** - * @brief Map an image \param I to a representation \param s. + * @brief Map an image \ref I to a representation \ref s. * This representation s has getProjectionSize() rows. * * Note that when combined with vpFeatureLuminanceMapping, - * The image \param I does not have the same size as the image input of vpFeatureLuminanceMapping::buildFrom. - * \param I is the center crop of this image. + * The image \ref I does not have the same size as the image input of vpFeatureLuminanceMapping::buildFrom. + * \ref 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 \param s + * @brief Compute the interaction matrix associated with the representation \ref s * * @param I input image used to compute s - * @param LI Photometric interaction matrix associated to \param I (see vpFeatureLuminance) - * @param s the already representation + * @param LI Photometric interaction matrix associated to \ref 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 \param I from a representation \param s + * @brief Reconstruct \ref I from a representation \ref s * * @param s the representation * @param I Output lossy reconstruction @@ -91,6 +91,8 @@ class VISP_EXPORT vpLuminanceMapping */ unsigned int getProjectionSize() const { return m_mappingSize; } + static void imageAsVector(const vpImage &I, vpColVector &Ivec, unsigned border); + protected: unsigned m_mappingSize; }; @@ -162,19 +164,68 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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; - std::shared_ptr m_mean; - vpColVector m_explainedVariance; + 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; }; class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature @@ -208,15 +259,11 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature 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; diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index d81e3ae387..ce86c8e584 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -4,6 +4,27 @@ #include #endif +// 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[i] = (double)I[i][j]; + } + } + +} + +// vpLuminancePCA + vpLuminancePCA::vpLuminancePCA(const std::shared_ptr &basis, const std::shared_ptr &mean, const vpColVector &explainedVariance) : vpLuminanceMapping(basis->getRows()) { @@ -165,6 +186,8 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int } +// Feature luminance mapping + vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpCameraParameters &cam, unsigned int h, unsigned int w, double Z, std::shared_ptr mapping) { @@ -219,11 +242,20 @@ vpFeatureLuminanceMapping *vpFeatureLuminanceMapping::duplicate() const } -void vpFeatureLuminanceMapping::buildFrom(vpImage &I) { } +void vpFeatureLuminanceMapping::buildFrom(vpImage &I) +{ + m_featI.buildFrom(I); + m_featI.interaction(m_LI); + + +} void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const -{ } +{ + + +} void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { } From 6c9cf8c950d58b5277ffb06c313f2f2846617a93 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 20 Feb 2024 02:31:22 +0100 Subject: [PATCH 11/28] PCA servoing works --- example/direct-visual-servoing/CMakeLists.txt | 5 +- .../photometricMappingVisualServoing.cpp | 529 ++++++++++++++++++ modules/core/test/math/testSvd.cpp | 42 +- .../visual_features/vpFeatureLuminance.h | 2 + .../vpFeatureLuminanceMapping.h | 30 +- .../src/visual-feature/vpFeatureLuminance.cpp | 7 +- .../vpFeatureLuminanceMapping.cpp | 39 +- .../test/feature/testLuminanceMapping.cpp | 2 +- 8 files changed, 628 insertions(+), 28 deletions(-) create mode 100644 example/direct-visual-servoing/photometricMappingVisualServoing.cpp diff --git a/example/direct-visual-servoing/CMakeLists.txt b/example/direct-visual-servoing/CMakeLists.txt index 16ef2260ca..8bc24e34dd 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_io 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 photometricVisualServoingWithoutVpServo -c -n 20 -m dct ${OPTION_TO_DESACTIVE_DISPLAY}) +visp_add_test(photometricMappingDCTVisualServoing photometricVisualServoingWithoutVpServo -c -n 20 -m pca -p 200 ${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..dda051c2a4 --- /dev/null +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -0,0 +1,529 @@ +/**************************************************************************** + * + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 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 photometricVisualServoing.cpp + + Implemented from \cite Collewet08c. +*/ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + + +#include +#include + +// List of allowed command line options +#define GETOPTARGS "cdi:n:p:m:k:h" + +void usage(const char *name, const char *badparam, std::string ipath, int niter, const std::string &method, unsigned numDbImages, const unsigned numComponents); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents); + +/*! + + 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) +{ + 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\ + -h\n\ + Print the help.\n", + ipath.c_str(), method.c_str(), numComponents, numDbImages, niter); + + 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) +{ + 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 'h': + usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents); + return false; + + default: + usage(argv[0], optarg_, ipath, niter, method, numDbImages, numComponents); + return false; + } + } + + if ((c == 1) || (c == -1)) { + // standalone param or error + usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents); + 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)) + 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 = "pca"; + unsigned opt_numDbImages = 2000; + unsigned opt_numComponents = 32; + + const double Z = 1.0; + 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) == 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); + 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.init(Itexture, X); + // ---------------------------------------------------------- + // Create the framegraber (here a simulated image) + vpImage I(ih, iw, 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_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)"); + } + std::vector> images(opt_numDbImages); + for (unsigned i = 0; i < opt_numDbImages; ++i) { + vpColVector to(3, 0.0), positionNoise(3, 0.0); + positionNoise[0] = random.uniform(-scenew / 4, scenew / 4); + positionNoise[1] = random.uniform(-sceneh / 4, sceneh / 4); + positionNoise[2] = random.uniform(-Z / 4.0, Z / 4.0); + + to[0] = random.uniform(-scenew / 4, scenew / 4); + to[1] = random.uniform(-sceneh / 4, sceneh / 4); + + const vpColVector from = cdMo.getTranslationVector() + positionNoise; + vpHomogeneousMatrix dbMo = vpMath::lookAt(from, to, 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 + vpFeatureLuminance temp; + vpLuminancePCA pca = vpLuminancePCA::learn(images, opt_numComponents, temp.getBorder()); + sMapping = std::shared_ptr(new vpLuminancePCA(pca)); + sdMapping = std::shared_ptr(new vpLuminancePCA(pca)); + } + else if (opt_method == "dct") { + throw vpException(vpException::badValue, "DCT not yet implemented!"); + } + 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; + + // 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 + + // ---------------------------------------------------------- + // position the robot at the initial position + // ---------------------------------------------------------- + + // camera desired position + vpHomogeneousMatrix cMo; + cMo.buildFrom(0, 0, 1.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(20)); + 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_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); + + // Affiche de l'image de difference +#if defined(VISP_HAVE_X11) + vpDisplayX d1; +#elif defined(VISP_HAVE_GDI) + vpDisplayGDI d1; +#elif defined(VISP_HAVE_GTK) + vpDisplayGTK d1; +#endif +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) + if (opt_display) { + d1.init(Idiff, 40 + static_cast(I.getWidth()), 10, "photometric visual servoing : s-s* "); + vpDisplay::display(Idiff); + vpDisplay::flush(Idiff); + } +#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 + std::cout << "Building sI" << std::endl; + vpFeatureLuminance luminanceI; + luminanceI.init(I.getHeight(), I.getWidth(), Z); + luminanceI.setCameraParameters(cam); + vpFeatureLuminanceMapping sI(luminanceI, sMapping); + std::cout << "Before build from" << std::endl; + sI.buildFrom(I); + + std::cout << "Building sId" << std::endl; + + // desired visual feature built from the image + vpFeatureLuminance luminanceId; + luminanceId.init(I.getHeight(), I.getWidth(), Z); + luminanceId.setCameraParameters(cam); + vpFeatureLuminanceMapping sId(luminanceId, sdMapping); + sId.buildFrom(Id); + + // Create visual-servoing task + vpServo servo; + // define the task + // - we want an eye-in-hand control law + // - robot is controlled in the camera frame + servo.setServo(vpServo::EYEINHAND_CAMERA); + // add current and desired visual features + servo.addFeature(sI, sId); + // set the gain + servo.setLambda(1); + // compute interaction matrix at the desired position + servo.setInteractionMatrixType(vpServo::CURRENT); + // set a velocity control mode + robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); + + int iter = 1; + double normError = 0; + vpColVector v; // camera velocity sent to the robot + + 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); +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) + if (opt_display) { + vpDisplay::display(I); + vpDisplay::flush(I); + } +#endif + vpImageTools::imageDifference(I, Id, Idiff); +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) + if (opt_display) { + vpDisplay::display(Idiff); + vpDisplay::flush(Idiff); + } +#endif + // Compute current visual feature + sI.buildFrom(I); + + v = servo.computeControlLaw(); // camera velocity send to the robot + + normError = servo.getError().sumSquare(); + std::cout << " |e| = " << normError << std::endl; + std::cout << " |v| = " << sqrt(v.sumSquare()) << std::endl; + + // 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); + + 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/modules/core/test/math/testSvd.cpp b/modules/core/test/math/testSvd.cpp index 3c7f9420da..5ac5bca339 100644 --- a/modules/core/test/math/testSvd.cpp +++ b/modules/core/test/math/testSvd.cpp @@ -235,8 +235,7 @@ void create_bench_random_symmetric_matrix(unsigned int nb_matrices, unsigned int std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_rows << " symmetric matrices" - << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_rows << " symmetric matrices" << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M; @@ -267,8 +266,11 @@ int test_svd(std::vector M, std::vector U, std::vector 1e-6) { - std::cout << "SVD decomposition failed" << std::endl; + double error = D.frobeniusNorm(); + if (error > 1e-6) { + std::cout << "SVD decomposition failed. Error: " << error << std::endl; + std::cout << "SVD decomposition failed. Mean error per component: " << error / D.size() << std::endl; + return EXIT_FAILURE; } } @@ -420,24 +422,25 @@ int main(int argc, const char *argv[]) if (use_plot_file) { of.open(plotfile.c_str()); of << "iter" - << "\t"; + << "\t"; #if defined(VISP_HAVE_LAPACK) of << "\"SVD Lapack\"" - << "\t"; + << "\t"; #endif #if defined(VISP_HAVE_EIGEN3) of << "\"SVD Eigen3\"" - << "\t"; + << "\t"; #endif #if defined(VISP_HAVE_OPENCV) of << "\"SVD OpenCV\"" - << "\t"; + << "\t"; #endif of << std::endl; } int ret = EXIT_SUCCESS; + int ret_test = 0; for (unsigned int iter = 0; iter < nb_iterations; iter++) { std::vector bench_random_matrices; create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices); @@ -449,22 +452,31 @@ int main(int argc, const char *argv[]) double time; #if defined(VISP_HAVE_LAPACK) - ret += test_svd_lapack(verbose, bench_random_matrices, time); + ret_test = test_svd_lapack(verbose, bench_random_matrices, time); + ret += ret_test; + std::cout << "SVD (Lapack) " << (ret_test ? "failed" : "succeed") << std::endl; save_time("SVD (Lapack): ", verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_EIGEN3) - ret += test_svd_eigen3(verbose, bench_random_matrices, time); + ret_test = test_svd_eigen3(verbose, bench_random_matrices, time); + ret += ret_test; + std::cout << "SVD (Eigen) " << (ret_test ? "failed" : "succeed") << std::endl; save_time("SVD (Eigen3): ", verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_OPENCV) - ret += test_svd_opencv(verbose, bench_random_matrices, time); + ret_test = test_svd_opencv(verbose, bench_random_matrices, time); + ret += ret_test; + std::cout << "SVD (OpenCV) " << (ret_test ? "failed" : "succeed") << std::endl; + save_time("SVD (OpenCV): ", verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_LAPACK) - ret += test_eigen_values_lapack(verbose, bench_random_symmetric_matrices, time); + ret_test = test_eigen_values_lapack(verbose, bench_random_symmetric_matrices, time); + ret += ret_test; + std::cout << "Eigen values (Lapack) " << (ret_test ? "failed" : "succeed") << std::endl; save_time("Eigen values (Lapack): ", verbose, use_plot_file, of, time); #endif @@ -478,7 +490,8 @@ int main(int argc, const char *argv[]) if (ret == EXIT_SUCCESS) { std::cout << "Test succeed" << std::endl; - } else { + } + else { std::cout << "Test failed" << std::endl; } @@ -489,7 +502,8 @@ int main(int argc, const char *argv[]) std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl; return EXIT_SUCCESS; #endif - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; return EXIT_FAILURE; } diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h index 2b0c891d94..2e023831d6 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h @@ -108,6 +108,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); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 772613ec75..34d819b5ec 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -56,6 +56,7 @@ class VISP_EXPORT vpLuminanceMapping * @param mappingSize The size of the space that this transformation maps to. */ vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } + /** * @brief Map an image \ref I to a representation \ref s. * This representation s has getProjectionSize() rows. @@ -67,6 +68,7 @@ class VISP_EXPORT vpLuminanceMapping * @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 \ref s * @@ -76,6 +78,7 @@ class VISP_EXPORT vpLuminanceMapping * @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 \ref I from a representation \ref s * @@ -91,10 +94,27 @@ class VISP_EXPORT vpLuminanceMapping */ 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); protected: - unsigned m_mappingSize; + unsigned m_mappingSize; //! Final vector size + unsigned m_border; //! Borders that were removed during raw photometric VS computation }; @@ -129,6 +149,14 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping */ 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 * diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 486d0162b2..1db0e7151c 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -127,11 +127,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); } @@ -164,6 +166,7 @@ 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(const vpCameraParameters &_cam) { cam = _cam; } @@ -184,7 +187,6 @@ void vpFeatureLuminance::buildFrom(vpImage &I) 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++) { double x = 0, y = 0; vpPixelMeterConversion::convertPoint(cam, j, i, x, y); @@ -201,14 +203,11 @@ void vpFeatureLuminance::buildFrom(vpImage &I) l = 0; for (unsigned int i = bord; i < nbr - bord; i++) { - // cout << i << endl ; for (unsigned int j = bord; j < nbc - bord; j++) { - // cout << dim_s <<" " < &I, vpColVec 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)); + 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[i] = (double)I[i][j]; + Ivec[l++] = (double)I[i][j]; } } - } // vpLuminancePCA @@ -45,9 +44,30 @@ void vpLuminancePCA::init(const std::shared_ptr &basis, const std::sha m_explainedVariance = variance; } -void vpLuminancePCA::map(const vpImage &I, vpColVector &s) + +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; + return *this; +} + +void vpLuminancePCA::map(const vpImage &I, vpColVector &s) +{ + imageAsVector(I, m_Ivec, m_border); + m_Ivec -= *m_mean; + s = (*m_basis) * m_Ivec; } void vpLuminancePCA::inverse(const vpColVector &s, vpImage &I) { @@ -153,6 +173,9 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int 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) { @@ -170,7 +193,7 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int vpColVector eigenValues; vpMatrix V; - centered.svdOpenCV(eigenValues, 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) { @@ -207,6 +230,7 @@ void vpFeatureLuminanceMapping::init() { dim_s = 0; m_featI.init(0, 0, 0.0); + m_mapping = nullptr; } void vpFeatureLuminanceMapping::init( @@ -216,6 +240,7 @@ void vpFeatureLuminanceMapping::init( 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); } @@ -224,6 +249,7 @@ void vpFeatureLuminanceMapping::init(const vpFeatureLuminance &luminance, std::s m_featI = luminance; m_mapping = mapping; dim_s = m_mapping->getProjectionSize(); + m_mapping->setBorder(m_featI.getBorder()); s.resize(dim_s, true); } @@ -246,8 +272,7 @@ void vpFeatureLuminanceMapping::buildFrom(vpImage &I) { m_featI.buildFrom(I); m_featI.interaction(m_LI); - - + m_mapping->map(I, s); } void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 953e05adab..335cd43310 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -59,7 +59,7 @@ SCENARIO("Using PCA features", "[visual_features]") GIVEN("A matrix containing simple data") { const unsigned numDataPoints = 200; - const unsigned int dataDim = 50; + const unsigned int dataDim = 500; const unsigned int trueComponents = 5; // Generate numDataPoints vectors in a "dataDim"-dimensional space. // The data is generated from "trueComponents" vectors, that are orthogonal From e43978b89d4039af943f622d26c2159b73deda33 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 20 Mar 2024 02:19:16 +0100 Subject: [PATCH 12/28] Start work on dct --- .../vpFeatureLuminanceMapping.h | 88 ++++++- .../vpFeatureLuminanceMapping.cpp | 246 +++++++++++++++++- .../test/feature/testLuminanceMapping.cpp | 170 +++++++++++- 3 files changed, 485 insertions(+), 19 deletions(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 34d819b5ec..af3348130e 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -111,6 +111,8 @@ class VISP_EXPORT vpLuminanceMapping 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 @@ -138,7 +140,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping { public: - vpLuminancePCA() : vpLuminanceMapping(0), m_basis(nullptr), m_mean(nullptr) { } + vpLuminancePCA() : vpLuminanceMapping(0), m_basis(nullptr), m_mean(nullptr), m_Ivec(0), m_Ih(0), m_Iw(0) { } /** * @brief Build a new PCA object @@ -253,9 +255,91 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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; + vpColVector m_Ivec; //! Vector representation of the image + unsigned int m_Ih, m_Iw; //! Input image dimensions (without borders); +}; + +class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping +{ +public: + + class 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; + }; + + vpLuminanceDCT() : vpLuminanceMapping(0) { } + + /** + * @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 Copy constructor + */ + vpLuminanceDCT(const vpLuminanceDCT &other); + + + vpLuminanceDCT &operator=(const vpLuminanceDCT &other); + + /** + * @brief Initialize the DCT object with the number of required components + */ + void init(const unsigned int k); + + 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 computeDCTMatrices(); + +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_D, m_Dt; //! the computed DCT matrix and its transpose. changed from original implementation: m_Dt is adapted to the width of the image (non square images) + vpLuminanceDCT::vpMatrixZigZagIndex m_zigzag; //! zigzag indexing helper + }; + class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature { diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index bb5ed60555..4048680e02 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -22,11 +22,27 @@ void vpLuminanceMapping::imageAsVector(const vpImage &I, vpColVec } } +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); } @@ -59,19 +75,31 @@ vpLuminancePCA &vpLuminancePCA::operator=(const vpLuminancePCA &other) 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 &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) @@ -84,9 +112,9 @@ vpLuminancePCA vpLuminancePCA::load(const std::string &basisFilename, const std: std::shared_ptr basis = std::make_shared(); std::shared_ptr mean = std::make_shared(); vpColVector explainedVariance; - vpMatrix::loadMatrix(basisFilename, *basis, false); - vpMatrix::loadMatrix(meanFilename, *mean, false); - vpMatrix::loadMatrix(explainedVarianceFile, explainedVariance, false); + vpMatrix::loadMatrix(basisFilename, *basis, true); + vpMatrix::loadMatrix(meanFilename, *mean, true); + vpMatrix::loadMatrix(explainedVarianceFile, explainedVariance, true); if (mean->getCols() > 1) { throw vpException(vpException::dimensionError, @@ -118,15 +146,13 @@ void vpLuminancePCA::save(const std::string &basisFilename, const std::string &m 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, false); - vpMatrix::saveMatrix(meanFilename, *m_mean, false); - vpMatrix::saveMatrix(explainedVarianceFile, m_explainedVariance, false); + 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]; @@ -145,6 +171,7 @@ vpLuminancePCA vpLuminancePCA::learn(const std::vector> & 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) { @@ -208,6 +235,207 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int 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; + unsigned int index = 0; + int row = 0, col = 0; + + bool row_inc = 0; + + unsigned mindim = std::min(rows, cols); + for (unsigned 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 == rows - 1) { + ++row; + } + else { + ++col; + } + row_inc = 1; + } + else { + if (row == cols - 1) { + ++col; + } + else { + ++row; + } + row_inc = 0; + } + + // Print the next half zig-zag pattern + int maxdim = std::max(rows, cols) - 1; + for (unsigned 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 == rows - 1) { + if (col == rows - 1) { + ++row; + } + else { + ++col; + } + row_inc = true; + } + + else if (col == 0 || row == cols - 1) { + if (row == cols - 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 + + +void vpLuminanceDCT::map(const vpImage &I, vpColVector &s) +{ + m_Ih = I.getHeight() - 2 * m_border; + m_Iw = I.getWidth() - 2 * m_border; + imageAsMatrix(I, m_Imat, m_border); + if (m_Imat.getCols() != m_Ih || m_Imat.getRows() != m_Iw) { + computeDCTMatrices(); + m_zigzag.init(m_Ih, m_Iw); + } + m_dct = m_D * m_Imat * m_Dt; + m_zigzag.getValues(m_dct, 0, m_mappingSize, s); +} + +void vpLuminanceDCT::computeDCTMatrices() +{ + m_D.resize(m_Ih, m_Iw, false, false); + for (unsigned j = 0; j < m_Ih; j++) + m_D[0][j] = 1/sqrt(m_Ih); + double alpha = sqrt(2./(m_Ih)); + for (unsigned int i = 1; i < m_Ih; i++) { + for (unsigned int j = 0; j < m_Iw; j++) { + m_D[i][j] = alpha*cos((2 * j + 1) * i * M_PI / (2 * m_Ih)); + } + } + + m_Dt.resize(m_Iw, m_Ih, false, false); + for (unsigned j = 0; j < m_Iw; j++) + m_Dt[j][0] = 1/sqrt(m_Iw); + alpha = sqrt(2. / m_Iw); + for (unsigned int i = 1; i < m_Iw; i++) { + for (unsigned int j = 0; j < m_Ih; j++) { + m_Dt[i][j] = alpha * cos((2 * j + 1) * i * M_PI / (2 * m_Iw)); + } + } +} + +void vpLuminanceDCT::inverse(const vpColVector &s, vpImage &I) +{ + +} + +void vpLuminanceDCT::interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) +{ + +} + // Feature luminance mapping diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 335cd43310..fb58a70920 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -58,18 +58,25 @@ SCENARIO("Using PCA features", "[visual_features]") { GIVEN("A matrix containing simple data") { - const unsigned numDataPoints = 200; - const unsigned int dataDim = 500; - const unsigned int trueComponents = 5; + 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); // dataDim x dataDim + 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(-1.0, 1.0); + 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; } } @@ -87,7 +94,7 @@ SCENARIO("Using PCA features", "[visual_features]") { for (unsigned int k = 1; k <= trueComponents; ++k) { - const vpLuminancePCA pca = vpLuminancePCA::learn(data.transpose(), k); + vpLuminancePCA pca = vpLuminancePCA::learn(data.transpose(), k); const vpMatrix &basis = *pca.getBasis(); THEN("Basis has correct dimensions") @@ -124,7 +131,6 @@ SCENARIO("Using PCA features", "[visual_features]") 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); @@ -144,13 +150,16 @@ SCENARIO("Using PCA features", "[visual_features]") } } 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; @@ -175,17 +184,162 @@ SCENARIO("Using PCA features", "[visual_features]") { 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 has correct dimensions") + 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()); + } + } + } + } + } +} +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)); + + } + } + } + + } + + } + int main(int argc, char *argv[]) { Catch::Session session; // There must be exactly one instance From 9f26f6b90a6bd4f82fac7d7fca4f63f4914d412f Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 22 Mar 2024 01:03:47 +0100 Subject: [PATCH 13/28] DCT working, example with lm control law --- example/direct-visual-servoing/CMakeLists.txt | 4 +- .../photometricMappingVisualServoing.cpp | 144 +++++++++++------- .../visual_features/vpFeatureLuminance.h | 2 + .../vpFeatureLuminanceMapping.h | 36 +++-- .../src/visual-feature/vpFeatureLuminance.cpp | 6 +- .../vpFeatureLuminanceMapping.cpp | 108 +++++++------ .../test/feature/testLuminanceMapping.cpp | 35 ++++- 7 files changed, 215 insertions(+), 120 deletions(-) diff --git a/example/direct-visual-servoing/CMakeLists.txt b/example/direct-visual-servoing/CMakeLists.txt index 8bc24e34dd..2e0cb6f1a4 100644 --- a/example/direct-visual-servoing/CMakeLists.txt +++ b/example/direct-visual-servoing/CMakeLists.txt @@ -58,5 +58,5 @@ visp_set_source_file_compile_flag(photometricMappingVisualServoing.cpp -Wno-stri # 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 photometricVisualServoingWithoutVpServo -c -n 20 -m dct ${OPTION_TO_DESACTIVE_DISPLAY}) -visp_add_test(photometricMappingDCTVisualServoing photometricVisualServoingWithoutVpServo -c -n 20 -m pca -p 200 ${OPTION_TO_DESACTIVE_DISPLAY}) +visp_add_test(photometricMappingPCAVisualServoing photometricMappingVisualServoing -c -n 200 -m pca -k 64 -p 250 -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 index dda051c2a4..509c44e2df 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -54,7 +54,6 @@ #include #include -#include #include #include @@ -64,10 +63,10 @@ #include // List of allowed command line options -#define GETOPTARGS "cdi:n:p:m:k:h" +#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); -bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents); +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); /*! @@ -79,7 +78,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all \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) +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\ @@ -121,10 +120,13 @@ OPTIONS: Default\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); + ipath.c_str(), method.c_str(), numComponents, numDbImages, niter, lambda); if (badparam) fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); @@ -144,7 +146,7 @@ OPTIONS: Default\n\ */ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, - int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents) + int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents, double &lambda) { const char *optarg_; int c; @@ -172,19 +174,22 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all case 'n': niter = atoi(optarg_); break; + case 'l': + lambda = atof(optarg_); + break; case 'h': - usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents); + usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents, lambda); return false; default: - usage(argv[0], optarg_, ipath, niter, method, numDbImages, numComponents); + 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); + 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; @@ -204,11 +209,17 @@ int main(int argc, const char **argv) bool opt_click_allowed = true; bool opt_display = true; int opt_niter = 400; - std::string opt_method = "pca"; + 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 = 1.0; + + const double Z = 0.8; const unsigned ih = 240; const unsigned iw = 320; const double scenew = 0.6; @@ -223,7 +234,8 @@ int main(int argc, const char **argv) 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) == false) { + 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; } @@ -244,7 +256,7 @@ int main(int argc, const char **argv) // 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); + 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 @@ -283,10 +295,13 @@ int main(int argc, const char **argv) 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; @@ -316,15 +331,16 @@ int main(int argc, const char **argv) std::vector> images(opt_numDbImages); for (unsigned i = 0; i < opt_numDbImages; ++i) { vpColVector to(3, 0.0), positionNoise(3, 0.0); - positionNoise[0] = random.uniform(-scenew / 4, scenew / 4); - positionNoise[1] = random.uniform(-sceneh / 4, sceneh / 4); - positionNoise[2] = random.uniform(-Z / 4.0, Z / 4.0); - - to[0] = random.uniform(-scenew / 4, scenew / 4); - to[1] = random.uniform(-sceneh / 4, sceneh / 4); - + 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); + double noiseDivTo = 16.0; + to[0] = random.uniform(-scenew / noiseDivTo, scenew / noiseDivTo); + to[1] = random.uniform(-sceneh / noiseDivTo, sceneh / noiseDivTo); const vpColVector from = cdMo.getTranslationVector() + positionNoise; - vpHomogeneousMatrix dbMo = vpMath::lookAt(from, to, vpColVector({ 0.0, 1.0, 0.0 })); + 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; @@ -335,13 +351,14 @@ int main(int argc, const char **argv) } 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 - vpFeatureLuminance temp; - vpLuminancePCA pca = vpLuminancePCA::learn(images, opt_numComponents, temp.getBorder()); + 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") { - throw vpException(vpException::badValue, "DCT not yet implemented!"); + 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!"); @@ -383,7 +400,7 @@ int main(int argc, const char **argv) // camera desired position vpHomogeneousMatrix cMo; - cMo.buildFrom(0, 0, 1.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(20)); + cMo.buildFrom(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 @@ -410,17 +427,22 @@ int main(int argc, const char **argv) // Affiche de l'image de difference #if defined(VISP_HAVE_X11) - vpDisplayX d1; + vpDisplayX d1, d2; + #elif defined(VISP_HAVE_GDI) - vpDisplayGDI d1; + vpDisplayGDI d1, d2; #elif defined(VISP_HAVE_GTK) - vpDisplayGTK d1; + vpDisplayGTK d1, d2; #endif #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) 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) @@ -442,6 +464,7 @@ int main(int argc, const char **argv) vpFeatureLuminanceMapping sI(luminanceI, sMapping); std::cout << "Before build from" << std::endl; sI.buildFrom(I); + sI.getMapping()->inverse(sI.get_s(), Irec); std::cout << "Building sId" << std::endl; @@ -452,24 +475,20 @@ int main(int argc, const char **argv) vpFeatureLuminanceMapping sId(luminanceId, sdMapping); sId.buildFrom(Id); - // Create visual-servoing task - vpServo servo; - // define the task - // - we want an eye-in-hand control law - // - robot is controlled in the camera frame - servo.setServo(vpServo::EYEINHAND_CAMERA); - // add current and desired visual features - servo.addFeature(sI, sId); - // set the gain - servo.setLambda(1); - // compute interaction matrix at the desired position - servo.setInteractionMatrixType(vpServo::CURRENT); // 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(); @@ -480,27 +499,41 @@ int main(int argc, const char **argv) // Acquire the new image sim.setCameraPosition(cMo); sim.getImage(I, cam); + vpImageTools::imageDifference(I, Id, Idiff); + + // Compute current visual features + sI.buildFrom(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); - } -#endif - vpImageTools::imageDifference(I, Id, Idiff); -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) - if (opt_display) { + vpDisplay::display(Irec); + vpDisplay::flush(Irec); vpDisplay::display(Idiff); vpDisplay::flush(Idiff); } #endif - // Compute current visual feature - sI.buildFrom(I); - - v = servo.computeControlLaw(); // camera velocity send to the robot - - normError = servo.getError().sumSquare(); - std::cout << " |e| = " << normError << std::endl; - std::cout << " |v| = " << sqrt(v.sumSquare()) << std::endl; // send the robot velocity robot.setVelocity(vpRobot::CAMERA_FRAME, v); @@ -514,6 +547,9 @@ int main(int argc, const char **argv) v = 0; robot.setVelocity(vpRobot::CAMERA_FRAME, v); + if (normError > 200) { + return EXIT_FAILURE; + } return EXIT_SUCCESS; } catch (const vpException &e) { diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h index 2e023831d6..f177fe4076 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h @@ -124,6 +124,8 @@ class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature 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 index af3348130e..95279240b9 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -301,14 +301,26 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping unsigned m_cols; }; - vpLuminanceDCT() : vpLuminanceMapping(0) { } /** * @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); } + 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 = 10; + m_Ih = m_Iw = 0; + } /** * @brief Copy constructor @@ -318,23 +330,21 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping vpLuminanceDCT &operator=(const vpLuminanceDCT &other); - /** - * @brief Initialize the DCT object with the number of required components - */ - void init(const unsigned int k); 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 computeDCTMatrices(); + 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_D, m_Dt; //! the computed DCT matrix and its transpose. changed from original implementation: m_Dt is adapted to the width of the image (non square images) + 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 }; @@ -359,10 +369,12 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature void buildFrom(vpImage &I); - void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, - unsigned int thickness = 1) const vp_override; - void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, - unsigned int thickness = 1) const vp_override; + 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; diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 1db0e7151c..bd5103ef51 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -50,6 +50,8 @@ For more details see \cite Collewet08c. */ +const int vpFeatureLuminance::DEFAULT_BORDER = 10; + /*! Initialize the memory space requested for vpFeatureLuminance visual feature. */ @@ -95,7 +97,7 @@ 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; @@ -111,7 +113,7 @@ vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixIn 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; } diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 4048680e02..f1ee19d9f1 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -102,7 +102,7 @@ void vpLuminancePCA::inverse(const vpColVector &s, vpImage &I) } } -void vpLuminancePCA::interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) +void vpLuminancePCA::interaction(const vpImage &, const vpMatrix &LI, const vpColVector &, vpMatrix &L) { L = (*m_basis) * LI; } @@ -236,10 +236,7 @@ vpLuminancePCA vpLuminancePCA::learn(const vpMatrix &images, const unsigned int } //vpMatrixZigZagIndex -vpLuminanceDCT::vpMatrixZigZagIndex::vpMatrixZigZagIndex() -{ - -} +vpLuminanceDCT::vpMatrixZigZagIndex::vpMatrixZigZagIndex() { } void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) { @@ -248,13 +245,16 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned 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; - unsigned mindim = std::min(rows, cols); - for (unsigned int len = 1; len <= mindim; ++len) { + 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; @@ -285,7 +285,7 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) // Update the indexes of row and col variable if (row == 0) { - if (col == rows - 1) { + if (col == rowCount - 1) { ++row; } else { @@ -294,7 +294,7 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) row_inc = 1; } else { - if (row == cols - 1) { + if (row == colCount - 1) { ++col; } else { @@ -304,8 +304,8 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) } // Print the next half zig-zag pattern - int maxdim = std::max(rows, cols) - 1; - for (unsigned len, diag = maxdim; diag > 0; --diag) { + int maxdim = std::max(rowCount, rowCount) - 1; + for (int len, diag = maxdim; diag > 0; --diag) { if (diag > mindim) { len = mindim; @@ -333,8 +333,8 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) } } - if (row == 0 || col == rows - 1) { - if (col == rows - 1) { + if (row == 0 || col == rowCount - 1) { + if (col == rowCount - 1) { ++row; } else { @@ -343,8 +343,8 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::init(unsigned rows, unsigned cols) row_inc = true; } - else if (col == 0 || row == cols - 1) { - if (row == cols - 1) { + else if (col == 0 || row == colCount - 1) { + if (row == colCount - 1) { ++col; } else { @@ -394,46 +394,69 @@ void vpLuminanceDCT::map(const vpImage &I, vpColVector &s) { m_Ih = I.getHeight() - 2 * m_border; m_Iw = I.getWidth() - 2 * m_border; - imageAsMatrix(I, m_Imat, m_border); if (m_Imat.getCols() != m_Ih || m_Imat.getRows() != m_Iw) { - computeDCTMatrices(); + computeDCTMatrices(m_Ih, m_Iw); m_zigzag.init(m_Ih, m_Iw); } - m_dct = m_D * m_Imat * m_Dt; + 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::computeDCTMatrices() + + +void vpLuminanceDCT::computeDCTMatrix(vpMatrix &D, unsigned int n) const { - m_D.resize(m_Ih, m_Iw, false, false); - for (unsigned j = 0; j < m_Ih; j++) - m_D[0][j] = 1/sqrt(m_Ih); - double alpha = sqrt(2./(m_Ih)); - for (unsigned int i = 1; i < m_Ih; i++) { - for (unsigned int j = 0; j < m_Iw; j++) { - m_D[i][j] = alpha*cos((2 * j + 1) * i * M_PI / (2 * m_Ih)); - } + D.resize(n, n, false, false); + for (unsigned i = 0; i < n; i++) { + D[0][i] = 1.0 / sqrt(n); } - - m_Dt.resize(m_Iw, m_Ih, false, false); - for (unsigned j = 0; j < m_Iw; j++) - m_Dt[j][0] = 1/sqrt(m_Iw); - alpha = sqrt(2. / m_Iw); - for (unsigned int i = 1; i < m_Iw; i++) { - for (unsigned int j = 0; j < m_Ih; j++) { - m_Dt[i][j] = alpha * cos((2 * j + 1) * i * M_PI / (2 * m_Iw)); + 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::inverse(const vpColVector &s, vpImage &I) +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 &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) +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]; + } + } } @@ -449,7 +472,7 @@ vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminance &l { init(luminance, mapping); } -vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminanceMapping &f) +vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpFeatureLuminanceMapping &f) : vpBasicFeature() { *this = f; } @@ -503,15 +526,6 @@ void vpFeatureLuminanceMapping::buildFrom(vpImage &I) m_mapping->map(I, s); } -void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, - unsigned int thickness) const -{ - - -} -void vpFeatureLuminanceMapping::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, - unsigned int thickness) const -{ } vpColVector vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, unsigned int select) diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index fb58a70920..564382d7f9 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -331,13 +331,42 @@ SCENARIO("Using DCT features", "[visual_features]") } } - } - } - + 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)); + } + //dct.interaction(I, ); + 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])); + if (diff > 2) { + FAIL(); + } + } + } + } + } + } } int main(int argc, char *argv[]) From dd8238dcb9fff4a27c2a005a3d8a41d398ed77e4 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Mon, 25 Mar 2024 19:20:01 +0100 Subject: [PATCH 14/28] Add documentation, fix doxygen warnings --- .../photometricMappingVisualServoing.cpp | 6 +-- .../vpFeatureLuminanceMapping.h | 40 ++++++++++++++----- .../vpFeatureLuminanceMapping.cpp | 3 +- 3 files changed, 34 insertions(+), 15 deletions(-) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp index 509c44e2df..e800d22823 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -31,9 +31,9 @@ *****************************************************************************/ /*! - \example photometricVisualServoing.cpp + \example photometricMappingVisualServoing.cpp - Implemented from \cite Collewet08c. + Implemented from \cite Collewet08c, \cite Marchand19a and \cite Marchand20a. */ #include @@ -364,8 +364,6 @@ int main(int argc, const char **argv) 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 diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 95279240b9..b02d79e545 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -34,6 +34,7 @@ #ifndef vpFeatureLuminanceMapping_h #define vpFeatureLuminanceMapping_h +#include #include #include @@ -41,6 +42,7 @@ #include #include + /** * @brief Base class for functions that map an image and its interaction matrix to a different domain. * * The mapping\f$ \mathbf{I} \rightarrow \mathbf{z}\f$ is done via vpLuminanceMapping::map @@ -58,29 +60,29 @@ class VISP_EXPORT vpLuminanceMapping vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } /** - * @brief Map an image \ref I to a representation \ref s. + * @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 \ref I does not have the same size as the image input of vpFeatureLuminanceMapping::buildFrom. - * \ref I is the center crop of this image. + * The image \p I does not have the same size as the image input of vpFeatureLuminanceMapping::buildFrom. + * \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 \ref s + * @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 \ref I (see vpFeatureLuminance) + * @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 \ref I from a representation \ref s + * @brief Reconstruct \p I from a representation \p s * * @param s the representation * @param I Output lossy reconstruction @@ -121,7 +123,7 @@ class VISP_EXPORT vpLuminanceMapping /** - * @brief Implementation of \cite{Marchand19a}. + * @brief Implementation of \cite Marchand19a. * * Projects an image onto an orthogonal subspace, * obtained via Principal Component Analysis (PCA). @@ -259,10 +261,20 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping unsigned int m_Ih, m_Iw; //! Input image dimensions (without borders); }; +/** + * @brief Implementation of \cite Marchand20a. + * + * 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 vpMatrixZigZagIndex { public: @@ -318,7 +330,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping void init(const unsigned int k) { m_mappingSize = k; - m_border = 10; + m_border = vpFeatureLuminance::DEFAULT_BORDER; m_Ih = m_Iw = 0; } @@ -349,10 +361,18 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping }; - +/** + * @brief Class to combine luminance features (photometric servoing) + * 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); diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index f1ee19d9f1..251fc497c4 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -58,6 +58,7 @@ void vpLuminancePCA::init(const std::shared_ptr &basis, const std::sha m_basis = basis; m_mean = mean; m_explainedVariance = variance; + m_border = vpFeatureLuminance::DEFAULT_BORDER; } @@ -66,7 +67,6 @@ vpLuminancePCA::vpLuminancePCA(const vpLuminancePCA &other) : vpLuminanceMapping *this = other; } - vpLuminancePCA &vpLuminancePCA::operator=(const vpLuminancePCA &other) { m_basis = other.m_basis; @@ -90,6 +90,7 @@ void vpLuminancePCA::map(const vpImage &I, vpColVector &s) 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)); From 0dffb03dc9756b6e4f8b13cfc65a2105ea416233 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 21 May 2024 15:43:10 +0200 Subject: [PATCH 15/28] Update test values to take less time --- example/direct-visual-servoing/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example/direct-visual-servoing/CMakeLists.txt b/example/direct-visual-servoing/CMakeLists.txt index 2e0cb6f1a4..6fdce810bf 100644 --- a/example/direct-visual-servoing/CMakeLists.txt +++ b/example/direct-visual-servoing/CMakeLists.txt @@ -58,5 +58,5 @@ visp_set_source_file_compile_flag(photometricMappingVisualServoing.cpp -Wno-stri # 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 250 -l 30.0 ${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}) From 5cac988dcc63aea84165271209444f891e2b7595 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 22 May 2024 13:41:40 +0200 Subject: [PATCH 16/28] Try and fix ZigZag library export on Windows --- .../include/visp3/visual_features/vpFeatureLuminanceMapping.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index b02d79e545..1de17fe7ab 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -275,7 +275,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping * @brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. * */ - class vpMatrixZigZagIndex + class VISP_EXPORT vpMatrixZigZagIndex { public: vpMatrixZigZagIndex(); From c8e76e3431bdf72440272de4239c50b2efa5be20 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 22 May 2024 14:19:38 +0200 Subject: [PATCH 17/28] rename buildFrom to build --- .../photometricMappingVisualServoing.cpp | 10 +++++----- .../visp3/visual_features/vpFeatureLuminanceMapping.h | 4 ++-- .../src/visual-feature/vpFeatureLuminanceMapping.cpp | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp index 67ca017e9d..7be2076b22 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -331,11 +331,11 @@ int main(int argc, const char **argv) std::vector> images(opt_numDbImages); for (unsigned i = 0; i < opt_numDbImages; ++i) { vpColVector to(3, 0.0), positionNoise(3, 0.0); - double noiseDiv = 16.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); - double noiseDivTo = 16.0; + 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; @@ -461,7 +461,7 @@ int main(int argc, const char **argv) luminanceI.setCameraParameters(cam); vpFeatureLuminanceMapping sI(luminanceI, sMapping); std::cout << "Before build from" << std::endl; - sI.buildFrom(I); + sI.build(I); sI.getMapping()->inverse(sI.get_s(), Irec); std::cout << "Building sId" << std::endl; @@ -471,7 +471,7 @@ int main(int argc, const char **argv) luminanceId.init(I.getHeight(), I.getWidth(), Z); luminanceId.setCameraParameters(cam); vpFeatureLuminanceMapping sId(luminanceId, sdMapping); - sId.buildFrom(Id); + sId.build(Id); // set a velocity control mode robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); @@ -500,7 +500,7 @@ int main(int argc, const char **argv) vpImageTools::imageDifference(I, Id, Idiff); // Compute current visual features - sI.buildFrom(I); + sI.build(I); sI.getMapping()->inverse(sI.get_s(), Irec); if (iter > iterGN) { diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 1de17fe7ab..b5ac0c8e9c 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -64,7 +64,7 @@ class VISP_EXPORT vpLuminanceMapping * 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::buildFrom. + * 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. @@ -387,7 +387,7 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature virtual ~vpFeatureLuminanceMapping() = default; - void buildFrom(vpImage &I); + void build(vpImage &I); void display(const vpCameraParameters &, const vpImage &, const vpColor & = vpColor::green, unsigned int = 1) const vp_override diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 251fc497c4..31b1ad6a93 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -520,9 +520,9 @@ vpFeatureLuminanceMapping *vpFeatureLuminanceMapping::duplicate() const } -void vpFeatureLuminanceMapping::buildFrom(vpImage &I) +void vpFeatureLuminanceMapping::build(vpImage &I) { - m_featI.buildFrom(I); + m_featI.build(I); m_featI.interaction(m_LI); m_mapping->map(I, s); } From a235ee2c2f320a469b1c84b77de647a00ec8abca Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 23 May 2024 16:53:24 +0200 Subject: [PATCH 18/28] Fix undefined symbols in vpLuminanceDCT --- .../visp3/visual_features/vpFeatureLuminanceMapping.h | 5 ++--- .../src/visual-feature/vpFeatureLuminanceMapping.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index b5ac0c8e9c..339a137b52 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -275,7 +275,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping * @brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. * */ - class VISP_EXPORT vpMatrixZigZagIndex + class vpMatrixZigZagIndex { public: vpMatrixZigZagIndex(); @@ -289,7 +289,6 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping /** * @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) @@ -340,7 +339,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping vpLuminanceDCT(const vpLuminanceDCT &other); - vpLuminanceDCT &operator=(const vpLuminanceDCT &other); + vpLuminanceDCT &operator=(const vpLuminanceDCT &other) = default; void map(const vpImage &I, vpColVector &s) vp_override; diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 31b1ad6a93..6b640e214a 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -390,6 +390,10 @@ void vpLuminanceDCT::vpMatrixZigZagIndex::setValues(const vpColVector &s, unsign // vpLuminanceDCT +vpLuminanceDCT::vpLuminanceDCT(const vpLuminanceDCT &other) : vpLuminanceMapping(other.getProjectionSize()) +{ + *this = other; +} void vpLuminanceDCT::map(const vpImage &I, vpColVector &s) { @@ -404,8 +408,6 @@ void vpLuminanceDCT::map(const vpImage &I, vpColVector &s) m_zigzag.getValues(m_dct, 0, m_mappingSize, s); } - - void vpLuminanceDCT::computeDCTMatrix(vpMatrix &D, unsigned int n) const { D.resize(n, n, false, false); From e6237a44ae33b4251a57ae60f93afa0b59527c6f Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 23 May 2024 17:15:16 +0200 Subject: [PATCH 19/28] GUI include guards for mapping vs example --- .../photometricMappingVisualServoing.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp index 7be2076b22..361579d0f5 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -316,6 +316,7 @@ int main(int argc, const char **argv) 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) @@ -328,6 +329,7 @@ int main(int argc, const char **argv) 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); @@ -368,7 +370,7 @@ int main(int argc, const char **argv) 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; @@ -390,6 +392,7 @@ int main(int argc, const char **argv) std::cout << "Click in the image to continue..." << std::endl; vpDisplay::getClick(I); } +#endif #endif // ---------------------------------------------------------- @@ -407,7 +410,7 @@ int main(int argc, const char **argv) I = 0; sim.getImage(I, cam); // and aquire the image Id -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) +#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); @@ -423,7 +426,8 @@ int main(int argc, const char **argv) vpImageTools::imageDifference(I, Id, Idiff); - // Affiche de l'image de difference + // Display image difference +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) #if defined(VISP_HAVE_X11) vpDisplayX d1, d2; @@ -432,7 +436,6 @@ int main(int argc, const char **argv) #elif defined(VISP_HAVE_GTK) vpDisplayGTK d1, d2; #endif -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) 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"); @@ -455,17 +458,13 @@ int main(int argc, const char **argv) // ------------------------------------------------------ // current visual feature built from the image - std::cout << "Building sI" << std::endl; vpFeatureLuminance luminanceI; luminanceI.init(I.getHeight(), I.getWidth(), Z); luminanceI.setCameraParameters(cam); vpFeatureLuminanceMapping sI(luminanceI, sMapping); - std::cout << "Before build from" << std::endl; sI.build(I); sI.getMapping()->inverse(sI.get_s(), Irec); - std::cout << "Building sId" << std::endl; - // desired visual feature built from the image vpFeatureLuminance luminanceId; luminanceId.init(I.getHeight(), I.getWidth(), Z); From e92e8e76b75c5cb7e89c6a723fb47490f3febf28 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 23 May 2024 17:24:44 +0200 Subject: [PATCH 20/28] reintroduce VISP_EXPORT for inner class --- .../include/visp3/visual_features/vpFeatureLuminanceMapping.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 339a137b52..9eef0d40d7 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -275,7 +275,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping * @brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. * */ - class vpMatrixZigZagIndex + class VISP_EXPORT vpMatrixZigZagIndex { public: vpMatrixZigZagIndex(); From 947174f067e3f9e61f4a0bfa450580ae55f5a6ae Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 23 May 2024 17:38:43 +0200 Subject: [PATCH 21/28] Add guard for C++ standard around newly introduced classes, which rely on shared ptr and other C++11 features --- .../photometricMappingVisualServoing.cpp | 2 +- .../include/visp3/visual_features/vpFeatureLuminanceMapping.h | 3 ++- .../src/visual-feature/vpFeatureLuminanceMapping.cpp | 4 +++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp index 361579d0f5..b60e597b3c 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -200,7 +200,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all int main(int argc, const char **argv) { -#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) +#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; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 9eef0d40d7..d19a757532 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -33,7 +33,7 @@ #ifndef vpFeatureLuminanceMapping_h #define vpFeatureLuminanceMapping_h - +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) #include #include @@ -417,3 +417,4 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature }; #endif +#endif diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index 6b640e214a..aafaf2b6fc 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -1,5 +1,5 @@ #include - +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) #ifdef VISP_HAVE_MODULE_IO #include #endif @@ -576,3 +576,5 @@ void vpFeatureLuminanceMapping::print(unsigned int select) const } std::cout << s << std::endl; } + +#endif // C++11 From e0a6595d40bab34894df5389940e7cedb82c98c2 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 23 May 2024 19:42:26 +0200 Subject: [PATCH 22/28] Fix include guard --- .../include/visp3/visual_features/vpFeatureLuminanceMapping.h | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index d19a757532..b3758411f3 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -33,6 +33,7 @@ #ifndef vpFeatureLuminanceMapping_h #define vpFeatureLuminanceMapping_h +#include #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) #include #include From b869ace3f9dec182a35260d85b544ffbed332564 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 24 May 2024 17:17:22 +0200 Subject: [PATCH 23/28] Rerunning tests with a bit more info --- .../visual_features/test/feature/testLuminanceMapping.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 564382d7f9..36ce3b8ed6 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -359,9 +359,8 @@ SCENARIO("Using DCT features", "[visual_features]") 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])); - if (diff > 2) { - FAIL(); - } + REQUIRE(diff < 2); + INFO("i = " + std::to_string(i) + ", j = " + std::to_string(j)); } } } From d001324372c5d378bde821d18a5d3504b35726bf Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 24 May 2024 18:06:56 +0200 Subject: [PATCH 24/28] Added a bit more testing on PCA --- .../test/feature/testLuminanceMapping.cpp | 43 ++++++++++++++++++- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 36ce3b8ed6..dfe07bc502 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -90,9 +90,17 @@ SCENARIO("Using PCA features", "[visual_features]") 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 = 1; k <= trueComponents; ++k) { + for (unsigned int k = 2; k <= trueComponents; ++k) { vpLuminancePCA pca = vpLuminancePCA::learn(data.transpose(), k); const vpMatrix &basis = *pca.getBasis(); @@ -123,7 +131,27 @@ SCENARIO("Using PCA features", "[visual_features]") 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"); @@ -226,6 +254,18 @@ SCENARIO("Using PCA features", "[visual_features]") } } } + 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]") @@ -352,7 +392,6 @@ SCENARIO("Using DCT features", "[visual_features]") REQUIRE(s.sum() == Approx(s[0]).margin(1e-5)); } - //dct.interaction(I, ); vpImage Ir; dct.inverse(s, Ir); REQUIRE((Ir.getRows() == I.getRows() && Ir.getCols() == I.getCols())); From 6564493f3590fccf09187fcc35ab54afa5ba3d11 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 28 May 2024 16:18:25 +0200 Subject: [PATCH 25/28] Fix Fallback version of matrix transpose with SimdLib for architectures that do not have avx --- .../simdlib/Simd/SimdBaseCustomFunctions.cpp | 420 +++++++++--------- 1 file changed, 212 insertions(+), 208 deletions(-) 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; + } +} +} } From 1ff185c2732207e911f6b36d9006d04847a70c0d Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 11 Jun 2024 14:56:29 +0200 Subject: [PATCH 26/28] Merge master and visp namespace --- .../photometricMappingVisualServoing.cpp | 16 +++---- .../vpFeatureLuminanceMapping.h | 12 ++--- .../vpFeatureLuminanceMapping.cpp | 48 +++++++++++++++---- .../test/feature/testLuminanceMapping.cpp | 5 ++ 4 files changed, 57 insertions(+), 24 deletions(-) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp index b60e597b3c..f116f24975 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp @@ -38,29 +38,29 @@ #include #include -#include - #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 -#include -#include // List of allowed command line options #define GETOPTARGS "cdi:n:p:m:k:hl:" diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index b3758411f3..8bdade4709 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -1,6 +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 @@ -31,8 +31,8 @@ * Luminance based feature. */ -#ifndef vpFeatureLuminanceMapping_h -#define vpFeatureLuminanceMapping_h +#ifndef VP_FEATURE_LUMINANCE_MAPPING_H +#define VP_FEATURE_LUMINANCE_MAPPING_H #include #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) #include @@ -43,14 +43,14 @@ #include #include - +BEGIN_VISP_NAMESPACE /** * @brief Base class for functions that map an image and its interaction matrix to a different domain. * * 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 + class VISP_EXPORT vpLuminanceMapping { public: /** @@ -416,6 +416,6 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature vpImage I; }; - +END_VISP_NAMESPACE #endif #endif diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp index aafaf2b6fc..5f24777c6e 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -1,8 +1,42 @@ +/* + * 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 @@ -534,7 +568,7 @@ void vpFeatureLuminanceMapping::build(vpImage &I) vpColVector vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, unsigned int select) { if (select != FEATURE_ALL) { - throw vpException(vpException::notImplementedError, "cannot compute error on subset of PCA features"); + throw vpException(vpException::notImplementedError, "cannot compute error on subset of a mapping"); } vpColVector e(dim_s); error(s_star, e); @@ -543,9 +577,6 @@ vpColVector vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, unsig void vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, vpColVector &e) { - // if (dim_s != s_star.dimension_s()) { - // throw vpException(vpException::dimensionError, "Wrong dimensions when computing error in PCA features"); - // } e.resize(dim_s, false); for (unsigned int i = 0; i < dim_s; i++) { e[i] = s[i] - s_star[i]; @@ -555,7 +586,7 @@ void vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, vpColVector vpMatrix vpFeatureLuminanceMapping::interaction(unsigned int select) { if (select != FEATURE_ALL) { - throw vpException(vpException::notImplementedError, "cannot compute interaction matrix for a subset of PCA features"); + throw vpException(vpException::notImplementedError, "cannot compute interaction matrix for a subset of a mapping"); } vpMatrix dsdr(dim_s, 6); interaction(dsdr); @@ -569,12 +600,9 @@ void vpFeatureLuminanceMapping::interaction(vpMatrix &L) } -void vpFeatureLuminanceMapping::print(unsigned int select) const +void vpFeatureLuminanceMapping::print(unsigned int /*select*/) const { - if (select != FEATURE_ALL) { - throw vpException(vpException::notImplementedError, "cannot print subset of PCA features"); - } 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 index dfe07bc502..23633a0e54 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -1,5 +1,6 @@ #include + #include #include @@ -9,6 +10,10 @@ #define CATCH_CONFIG_RUNNER #include +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + vpMatrix orthogonalBasis(unsigned n, unsigned seed) { From 91c8e0ce780acd6c2fdc20ccb0153cf964712462 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 11 Jun 2024 15:08:21 +0200 Subject: [PATCH 27/28] improve documentation --- .../vpFeatureLuminanceMapping.h | 81 +++++++++++-------- 1 file changed, 46 insertions(+), 35 deletions(-) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index 8bdade4709..e9cb48505e 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -44,24 +44,28 @@ #include BEGIN_VISP_NAMESPACE -/** - * @brief Base class for functions that map an image and its interaction matrix to a different domain. - * * 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 + +/*! + * \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 + * \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. + * \brief Map an image \p I to a representation \p s. * This representation s has getProjectionSize() rows. * * Note that when combined with vpFeatureLuminanceMapping, @@ -73,7 +77,7 @@ BEGIN_VISP_NAMESPACE virtual void map(const vpImage &I, vpColVector &s) = 0; /** - * @brief Compute the interaction matrix associated with the representation \p s + * \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) @@ -83,7 +87,7 @@ BEGIN_VISP_NAMESPACE virtual void interaction(const vpImage &I, const vpMatrix &LI, const vpColVector &s, vpMatrix &L) = 0; /** - * @brief Reconstruct \p I from a representation \p s + * \brief Reconstruct \p I from a representation \p s * * @param s the representation * @param I Output lossy reconstruction @@ -91,21 +95,21 @@ BEGIN_VISP_NAMESPACE virtual void inverse(const vpColVector &s, vpImage &I) = 0; /** - * @brief Returns the size of the space to which an image is mapped to. + * \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 + * \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 + * \brief Set the number of pixels that are removed by the photometric VS computation * This function should be called by vpFeatureLuminanceMapping * * @param border @@ -124,7 +128,9 @@ BEGIN_VISP_NAMESPACE /** - * @brief Implementation of \cite Marchand19a. + * \brief Implementation of \cite Marchand19a. + * + * \ingroup group_visual_features * * Projects an image onto an orthogonal subspace, * obtained via Principal Component Analysis (PCA). @@ -146,7 +152,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping vpLuminancePCA() : vpLuminanceMapping(0), m_basis(nullptr), m_mean(nullptr), m_Ivec(0), m_Ih(0), m_Iw(0) { } /** - * @brief Build a new PCA object + * \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 @@ -155,7 +161,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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 + * \brief Copy constructor: does not make a deep copy of the basis and mean */ vpLuminancePCA(const vpLuminancePCA &other); @@ -163,7 +169,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping vpLuminancePCA &operator=(const vpLuminancePCA &other); /** - * @brief Initialize the PCA object with a basis, mean and explained variance vector + * \brief Initialize the PCA object with a basis, mean and explained variance vector * * \sa vpLuminancePCA() * @param basis @@ -173,19 +179,19 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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$) + * \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. + * \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. + * \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. @@ -198,7 +204,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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. + * \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 @@ -209,7 +215,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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. + * \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 @@ -220,7 +226,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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. + * \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 @@ -233,7 +239,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping 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. + * \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 @@ -245,7 +251,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping */ 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 + * \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 @@ -263,7 +269,9 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping }; /** - * @brief Implementation of \cite Marchand20a. + * \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. @@ -273,7 +281,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping public: /** - * @brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. + * \brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. * */ class VISP_EXPORT vpMatrixZigZagIndex @@ -281,14 +289,14 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping public: vpMatrixZigZagIndex(); /** - * @brief Initalize the ZigZag object. Computes and stores the zigzag indexing for a given matrix size + * \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 + * \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 @@ -298,7 +306,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping 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 + * \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 @@ -315,7 +323,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping /** - * @brief Build a new DCT object + * \brief Build a new DCT object * * @param k the number of components to keep from the DCT matrix and use as servoing features */ @@ -325,7 +333,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping } /** - * @brief Initialize the DCT object with the number of required components + * \brief Initialize the DCT object with the number of required components */ void init(const unsigned int k) { @@ -335,7 +343,7 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping } /** - * @brief Copy constructor + * \brief Copy constructor */ vpLuminanceDCT(const vpLuminanceDCT &other); @@ -362,7 +370,10 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping }; /** - * @brief Class to combine luminance features (photometric servoing) + * \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) * From e9e69f18a807634c2422242be55e25692aa50166 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 11 Jun 2024 16:54:38 +0200 Subject: [PATCH 28/28] Improve doc, remove empty lines --- .../photometricMappingVisualServoing.cpp | 8 ++-- .../photometricVisualServoing.cpp | 10 ++--- ...hotometricVisualServoingWithoutVpServo.cpp | 10 ++--- .../vpFeatureLuminanceMapping.h | 16 +------- .../src/visual-feature/vpFeatureLuminance.cpp | 18 +++------ .../vpFeatureLuminanceMapping.cpp | 7 ---- .../test/feature/testLuminanceMapping.cpp | 39 ++++++++++++++++++- .../test/feature/testPoint.cpp | 15 ++++--- 8 files changed, 63 insertions(+), 60 deletions(-) diff --git a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp b/example/direct-visual-servoing/photometricMappingVisualServoing.cpp index f116f24975..7e50556f0c 100644 --- a/example/direct-visual-servoing/photometricMappingVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricMappingVisualServoing.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 photometricMappingVisualServoing.cpp 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/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index e9cb48505e..ae7573bcc5 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -116,17 +116,14 @@ class VISP_EXPORT vpLuminanceMapping */ 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. * @@ -148,7 +145,6 @@ class VISP_EXPORT vpLuminanceMapping 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) { } /** @@ -165,7 +161,6 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping */ vpLuminancePCA(const vpLuminancePCA &other); - vpLuminancePCA &operator=(const vpLuminancePCA &other); /** @@ -224,6 +219,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping * \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. @@ -238,6 +234,7 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping */ 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. * @@ -279,7 +276,6 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping { public: - /** * \brief Helper class to iterate and get/set the values from a matrix, following a zigzag pattern. * @@ -321,7 +317,6 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping unsigned m_cols; }; - /** * \brief Build a new DCT object * @@ -347,10 +342,8 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping */ 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; @@ -366,7 +359,6 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping 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 - }; /** @@ -385,7 +377,6 @@ class VISP_EXPORT vpLuminanceDCT : public vpLuminanceMapping 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; @@ -407,7 +398,6 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature 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); @@ -420,12 +410,10 @@ class VISP_EXPORT vpFeatureLuminanceMapping : public vpBasicFeature 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 diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 2082124cf1..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 @@ -112,7 +110,7 @@ vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(DEFAULT_BO } /*! - Copy constructor. + Copy constructor. */ vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance &f) : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam() @@ -121,7 +119,7 @@ vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance &f) } /*! - Copy operator. + Copy operator. */ vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance &f) { @@ -237,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$ */ @@ -379,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 index 5f24777c6e..e167e6d8d7 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminanceMapping.cpp @@ -95,7 +95,6 @@ void vpLuminancePCA::init(const std::shared_ptr &basis, const std::sha m_border = vpFeatureLuminance::DEFAULT_BORDER; } - vpLuminancePCA::vpLuminancePCA(const vpLuminancePCA &other) : vpLuminanceMapping(other.m_mappingSize) { *this = other; @@ -496,7 +495,6 @@ void vpLuminanceDCT::interaction(const vpImage &, const vpMatrix } } - // Feature luminance mapping vpFeatureLuminanceMapping::vpFeatureLuminanceMapping(const vpCameraParameters &cam, @@ -541,7 +539,6 @@ void vpFeatureLuminanceMapping::init(const vpFeatureLuminance &luminance, std::s s.resize(dim_s, true); } - vpFeatureLuminanceMapping &vpFeatureLuminanceMapping::operator=(const vpFeatureLuminanceMapping &f) { dim_s = f.dim_s; @@ -555,7 +552,6 @@ vpFeatureLuminanceMapping *vpFeatureLuminanceMapping::duplicate() const return new vpFeatureLuminanceMapping(*this); } - void vpFeatureLuminanceMapping::build(vpImage &I) { m_featI.build(I); @@ -563,8 +559,6 @@ void vpFeatureLuminanceMapping::build(vpImage &I) m_mapping->map(I, s); } - - vpColVector vpFeatureLuminanceMapping::error(const vpBasicFeature &s_star, unsigned int select) { if (select != FEATURE_ALL) { @@ -599,7 +593,6 @@ void vpFeatureLuminanceMapping::interaction(vpMatrix &L) m_mapping->interaction(I, m_LI, s, L); } - void vpFeatureLuminanceMapping::print(unsigned int /*select*/) const { std::cout << s << std::endl; diff --git a/modules/visual_features/test/feature/testLuminanceMapping.cpp b/modules/visual_features/test/feature/testLuminanceMapping.cpp index 23633a0e54..02b1a76d5e 100644 --- a/modules/visual_features/test/feature/testLuminanceMapping.cpp +++ b/modules/visual_features/test/feature/testLuminanceMapping.cpp @@ -1,4 +1,42 @@ +/* + * 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 @@ -14,7 +52,6 @@ using namespace VISP_NAMESPACE_NAME; #endif - vpMatrix orthogonalBasis(unsigned n, unsigned seed) { vpUniRand rand(seed); 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