diff --git a/.github/workflows/macos-ustk.yml b/.github/workflows/macos-ustk.yml index 7590fe1878..120d6211e2 100644 --- a/.github/workflows/macos-ustk.yml +++ b/.github/workflows/macos-ustk.yml @@ -13,7 +13,7 @@ jobs: strategy: fail-fast: false matrix: - os: [macos-11, macos-12] + os: [macos-12] steps: # https://github.com/marketplace/actions/cancel-workflow-action @@ -34,7 +34,7 @@ jobs: run: system_profiler SPSoftwareDataType - name: Install dependencies - run: brew install libpng libjpeg libdc1394 opencv pcl librealsense zbar vtk fftw armadillo nlohmann-json + run: brew install libpng libjpeg-turbo libdc1394 opencv pcl librealsense zbar pkg-config nlohmann-json - name: Clone visp-images env: diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 2f4e351f10..2e9ea34b43 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -13,7 +13,7 @@ jobs: strategy: fail-fast: false matrix: - os: [macos-11, macos-12] + os: [macos-12] steps: # https://github.com/marketplace/actions/cancel-workflow-action @@ -34,7 +34,7 @@ jobs: run: system_profiler SPSoftwareDataType - name: Install dependencies - run: brew install libpng libjpeg libdc1394 opencv pcl librealsense zbar pkg-config nlohmann-json + run: brew install libpng libjpeg-turbo libdc1394 opencv pcl librealsense zbar pkg-config nlohmann-json - name: Install java dependencies run: | @@ -87,18 +87,7 @@ jobs: make -j$(sysctl -n hw.logicalcpu) - name: ViSP as 3rdparty with visp.pc and pkg-config - if: matrix.os != 'macOS-10.15' run: | - # With macOS 10.15 there is the following build error: - # /Applications/Xcode_12.4.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/../include/c++/v1/cmath:317:9: - # error: no member named 'signbit' in the global namespace - # using ::signbit; - # also reported here https://stackoverflow.com/questions/58313047/cannot-compile-r-packages-with-c-code-after-updating-to-macos-catalina - # As suggested adding -isysroot build flag in VISPGenerateConfigScript.cmake around line 149 with - # execute_process(COMMAND xcrun --show-sdk-path - # OUTPUT_VARIABLE SDK_PATH OUTPUT_STRIP_TRAILING_WHITESPACE) - # list(APPEND _cxx_flags "-isysroot ${SDK_PATH}") - # doesn't fix the build issue. That's why here the test is only done for macOS 11.0 cd ${HOME}/visp_sample export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/tmp/usr/local/lib/pkgconfig pkg-config --cflags visp @@ -107,7 +96,6 @@ jobs: make -j$(sysctl -n hw.logicalcpu) -f Makefile.visp.pc clean - name: ViSP as 3rdparty with visp-config - if: matrix.os != 'macos-10.15' run: | cd ${HOME}/visp_sample export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/tmp/usr/local/lib/pkgconfig diff --git a/.github/workflows/ubuntu-dep-src.yml b/.github/workflows/ubuntu-dep-src.yml index c481c39a38..af526068c8 100644 --- a/.github/workflows/ubuntu-dep-src.yml +++ b/.github/workflows/ubuntu-dep-src.yml @@ -128,7 +128,14 @@ jobs: cmake .. -DCMAKE_INSTALL_PREFIX=$(pwd)/install cat ViSP-third-party.txt - - name: Compile + - name: Build visp-config script + working-directory: build + run: | + make -j$(nproc) developer_scripts + ./bin/visp-config --cflags + ./bin/visp-config --libs + + - name: Build and install ViSP working-directory: build run: | make -j$(nproc) install diff --git a/3rdparty/pololu/CMakeLists.txt b/3rdparty/pololu/CMakeLists.txt new file mode 100644 index 0000000000..d9740501e2 --- /dev/null +++ b/3rdparty/pololu/CMakeLists.txt @@ -0,0 +1,45 @@ +project(${POLOLU_LIBRARY}) + +set(POLOLU_MAJOR_VERSION 1 PARENT_SCOPE) +set(POLOLU_MINOR_VERSION 0 PARENT_SCOPE) +set(POLOLU_PATCH_VERSION 0 PARENT_SCOPE) + +vp_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +set(lib_srcs src/RPMSerialInterface.cpp) +set(lib_hdrs include/RPMSerialInterface.h) + +if(CMAKE_SYSTEM_NAME MATCHES "Windows") + list(APPEND lib_hdrs include/RPMSerialInterfaceWindows.h) + list(APPEND lib_srcs src/RPMSerialInterfaceWindows.cpp) +elseif( CMAKE_SYSTEM_NAME MATCHES "Linux" OR CMAKE_SYSTEM_NAME MATCHES "Darwin") + # Could also be used on Windows with MinGW + list(APPEND lib_hdrs include/RPMSerialInterfacePOSIX.h) + list(APPEND lib_srcs src/RPMSerialInterfacePOSIX.cpp) +else() + message("${PROJECT_NAME} is only available for Windows, Linux and Darwin") +endif() + +add_library(${POLOLU_LIBRARY} STATIC ${lib_srcs} ${lib_hdrs}) + +if(UNIX) + if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") + endif() +endif() + +if(ENABLE_SOLUTION_FOLDERS) + set_target_properties(${POLOLU_LIBRARY} PROPERTIES FOLDER "3rdparty") +endif() + +set_target_properties(${POLOLU_LIBRARY} + PROPERTIES OUTPUT_NAME ${POLOLU_LIBRARY} + DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}" + COMPILE_PDB_NAME ${POLOLU_LIBRARY} + COMPILE_PDB_NAME_DEBUG "${POLOLU_LIBRARY}${VISP_DEBUG_POSTFIX}" + ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH} + ) + +if(NOT BUILD_SHARED_LIBS) + vp_install_target(${POLOLU_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev) +endif() diff --git a/3rdparty/pololu/LICENSE b/3rdparty/pololu/LICENSE new file mode 100644 index 0000000000..258c580888 --- /dev/null +++ b/3rdparty/pololu/LICENSE @@ -0,0 +1,22 @@ +The MIT License (MIT) + +Copyright (c) 2015 Jacques Menuet + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + diff --git a/3rdparty/pololu/README.ViSP b/3rdparty/pololu/README.ViSP new file mode 100644 index 0000000000..3565803fbf --- /dev/null +++ b/3rdparty/pololu/README.ViSP @@ -0,0 +1,6 @@ +Porting of the RapaPololuMaestro project: https://github.com/jbitoniau/RapaPololuMaestro commit d30b0af210 + +2023-10-20: +- Changes introduced to fix warnings +- Remove the RPM namespace that makes the SerialInterface class nested and prevents it from being used as a private + class in vpServoPololuMaestro.h diff --git a/3rdparty/pololu/README.md b/3rdparty/pololu/README.md new file mode 100644 index 0000000000..2350cef7dd --- /dev/null +++ b/3rdparty/pololu/README.md @@ -0,0 +1,75 @@ +RapaPololuMaestro +================= +A cross-platform C++ library for controlling Pololu Maestro devices + +![alt text](docs/RapaPololuMaestro1.jpg?raw=true "Pololu Maestro driving a servo") +![alt text](docs/RapaPololuMaestro2.jpg?raw=true "Pololu Maestro controlled by a Raspberry Pi") +![alt text](docs/RapaPololuMaestroViewer.png?raw=true "Pololu Maestro Viewer") + +# Overview +RapaPololuMaestro (formerly [Polstro](https://code.google.com/p/polstro/)) is a lightweight cross-platform C++ library that provides access, in an object-oriented fashion, to one or more [Pololu Maestro servo-controller devices](http://www.pololu.com/docs/0J40) connected to the computer. + +The communication is based on the [Serial Interface protocol](http://www.pololu.com/docs/0J40/5.c Serial Interface), not the native USB interface (which offers more advanced functionalities). + +With RapaPololuMaestro it's possible to: + +* set/get the target position of any servo connected to the Maestro device (from 6 to 24 depending on the Maestro model) +* set the speed and acceleration at which the Maestro changes the position. + +Note that by servo here we mean any RC component that is driven by a PWM signal. This can be for example an ESC (Electronic Speed Controller) that controls a brushless motor. + +The code is based on the [cross-platform C](http://www.pololu.com/docs/0J40/5.h.2) and [Windows examples](http://www.pololu.com/docs/0J40/5.h.2 Windows) provided by Pololu. + +# Example of use +```cpp +#include "RPMSerialInterface.h" + +int main( int argc, char** argv ) +{ + // Create the interface + RPM::SerialInterface* serialInterface = RPM::SerialInterface::createSerialInterface( "COM4", 9600 ); + if ( !serialInterface->isOpen() ) + return -1; + + // Set the value of channel 0 to 6000 quarter-of-microseconds (i.e. 1.5 milliseconds) + serialInterface->setTargetCP( 0, 6000 ); + + // Delete the interface + delete serialInterface; + + return 0; +} +``` + +# Compiling the code +RapaPololuMaestro uses CMake 3 to create the build system corresponding to the platform. You need to have it installed on your system, please visit [CMake](http://www.cmake.org/) for more information. + +After downloading the source code and opening a console in the main source folder, creating the build system can be done like this: + +``` +mkdir _build_ +cd _build_ +cmake .. -DCMAKE_INSTALL_PREFIX=../_output_ +``` + +You should then have a build system ready to use for your platform (for example, a Visual Studio solution on Windows, a makefile on Linux, etc...). + +The build process generates a static library, and two samples: +* a command-line test program. +* a GUI program to control a Maestro interactively + +The GUI uses Qt as a dependency. If it can't be found on your system, the GUI program will simply be not built. +Either Qt4 or Qt5 can be used. You can specify one or the other using the RAPA_USE_QT5 CMake variable. For example, to compile using QT4: + +``` +cmake .. -DCMAKE_INSTALL_PREFIX=../_output_ -DRAPA_USE_QT5=OFF +``` + +If CMake doesn't find Qt automatically, you can point it to the appropriate folder. In this example we also compile for Visual Studio 12 2013 in 64 bits: + +``` +cmake .. -DCMAKE_INSTALL_PREFIX=../_output_ -DCMAKE_PREFIX_PATH=C:\SomePath\Qt\5.5\msvc2013_64 -G "Visual Studio 12 2013 Win64" +``` + +Once the compilation done, you can deploy/install RapaPololuMaestro as a standalone SDK, by building the INSTALL target/project generated by CMake. This will copy the headers, libs and binaries into the directory designated by the standard CMAKE_INSTALL_PREFIX variable. + diff --git a/3rdparty/pololu/include/RPMSerialInterface.h b/3rdparty/pololu/include/RPMSerialInterface.h new file mode 100644 index 0000000000..6648fd9630 --- /dev/null +++ b/3rdparty/pololu/include/RPMSerialInterface.h @@ -0,0 +1,130 @@ +/* + The MIT License (MIT) (http://opensource.org/licenses/MIT) + + Copyright (c) 2015 Jacques Menuet + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#pragma once + +#include + +/* + SerialInterface + + The SerialInterface implements the Pololu Maestro serial communication protocol. + See http://www.pololu.com/docs/0J40/5.e for the details of the commands available. + + The SerialInterface methods of the SerialInterface come into different flavours, + each uses a different protocol: + - Compact protocol (CP) + - Pololu protocol (PP) + - Mini-SSC protocol (MSSCP) + See http://www.pololu.com/docs/0J40/5.c for more information. + + Note: add the ability to specify min/max target value at construction time rather + than having it hard-coded. +*/ +class RPMSerialInterface +{ +public: + // Create a concrete platform-specific SerialInterface. + // Return NULL if the interface couldn't be created and set the optional error message. + static RPMSerialInterface *createSerialInterface(const std::string &portName, unsigned int baudRate, std::string *errorMessage = NULL); + + // Return the last error message. The message is set when a methods encounters a problem and returns false. + // It is automatically cleared at the beginning of each method. + const std::string &getErrorMessage() const { return mErrorMessage; } + + // Destructor + virtual ~RPMSerialInterface(); + + // Indicates whether the serial port has been successfully open at construction + virtual bool isOpen() const = 0; + + // Return the minimum valid channel value in 0.25 microsecond units + static unsigned short getMinChannelValue() { return mMinChannelValue; } + + // Return the maximum valid channel value in 0.25 microsecond units + static unsigned short getMaxChannelValue() { return mMaxChannelValue; } + + // Set the target position of a channel to a given value in 0.25 microsecond units + bool setTargetCP(unsigned char channelNumber, unsigned short target); + bool setTargetPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned short target); + + // In the Mini-SSC version of this method, the normalizedTarget value is between 0 and 255. + // The value is converted to an actual 0.25 microsecond unit value based on pre-configured + // settings (neutral and range parameters) stored on the Pololu Maestro device itself. + // This is done using a tool such as the Maestro Control Center. Calibration can be therefore + // done externally quite easily. + // Additionally, the miniSCC channel number allows access to channels of chained devices. + bool setTargetMSSCP(unsigned char miniSCCChannelNumber, unsigned char normalizedTarget); + + // On Mini Maestro 12, 18 and 24 only, so not supported here (as I only have a Mini Maestro 6) + // bool setMultipleTargets(...) + + // Set the speed limit of a channel in units of (0.25 microsecond)/(10ms) + bool setSpeedCP(unsigned char channelNumber, unsigned short speed); + bool setSpeedPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned short speed); + + // Set the acceleration limit of a channel in units of (0.25 microsecond)/(10ms)/(80ms) + bool setAccelerationCP(unsigned char channelNumber, unsigned char acceleration); + bool setAccelerationPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned char acceleration); + + // Return the position of a channel. + // For a servo channel, the position is the current pulse width in 0.25 microsecond units + // For a digital output channel, a position less than 6000 means the line is low, and high when above 6000 + // For an input channel, the position represents the voltage measured on the channel. + bool getPositionCP(unsigned char channelNumber, unsigned short &position); + bool getPositionPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned short &position); + + // Indicate whether the servo outputs have reached their targets or are still moving. + bool getMovingStateCP(bool &servosAreMoving); + bool getMovingStatePP(unsigned char deviceNumber, bool &servosAreMoving); + + // The return the error detected by the Pololu Maestro. This automatically clears the error flag. + bool getErrorsCP(unsigned short &error); + bool getErrorsPP(unsigned char deviceNumber, unsigned short &error); + + // Request the Pololu Maestro to "go home". + // Going home sets the channels to their startup/error state. + // This state is defined on a per-channel. It can either be: + // - ignore: the value is unchanged when we do a "go home". PWM signal is continues to be generated + // - go to: the channel is set to the specified value. Again PWM signal is generated + // - off: the channel is turned off. There's no more PWM signal generated for the channel + bool goHomeCP(); + bool goHomePP(unsigned char deviceNumber); + +protected: + RPMSerialInterface(); + void clearErrorMessage(); + void setErrorMessage(const std::string &message); + + bool checkPortIsOpen() const; // And update error message if not + bool checkValidTargetValue(unsigned short target) const; // Same here + +private: + static const unsigned short mMinChannelValue = 3968; + static const unsigned short mMaxChannelValue = 8000; + + virtual bool writeBytes(const unsigned char *data, unsigned int dataSizeInBytes) = 0; + virtual bool readBytes(unsigned char *data, unsigned int dataSizeInBytes) = 0; + + std::string mErrorMessage; +}; diff --git a/3rdparty/pololu/include/RPMSerialInterfacePOSIX.h b/3rdparty/pololu/include/RPMSerialInterfacePOSIX.h new file mode 100644 index 0000000000..c66ba3ee49 --- /dev/null +++ b/3rdparty/pololu/include/RPMSerialInterfacePOSIX.h @@ -0,0 +1,49 @@ +/* + The MIT License (MIT) (http://opensource.org/licenses/MIT) + + Copyright (c) 2015 Jacques Menuet + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#pragma once + +#include "RPMSerialInterface.h" + +class RPMSerialInterfacePOSIX : public RPMSerialInterface +{ +public: + // Creates a POSIX SerialInterface. + // The syntax of the port name depends on the platform. For example: + // - on Windows: "\\\\.\\USBSER000", "\\\\.\\COM6", etc... + // - on Linux: "/dev/ttyACM0" + // - on Mac OS: "/dev/cu.usbmodem00034567" + RPMSerialInterfacePOSIX(const std::string &portName, std::string *errorMessage = NULL); + + virtual ~RPMSerialInterfacePOSIX(); + + virtual bool isOpen() const; + +private: + int openPort(const std::string &portName, std::string *errorMessage = NULL); + + virtual bool writeBytes(const unsigned char *data, unsigned int dataSizeInBytes); + virtual bool readBytes(unsigned char *data, unsigned int dataSizeInBytes); + + int mFileDescriptor; +}; diff --git a/3rdparty/pololu/include/RPMSerialInterfaceWindows.h b/3rdparty/pololu/include/RPMSerialInterfaceWindows.h new file mode 100644 index 0000000000..60b6b5de6c --- /dev/null +++ b/3rdparty/pololu/include/RPMSerialInterfaceWindows.h @@ -0,0 +1,54 @@ +/* + The MIT License (MIT) (http://opensource.org/licenses/MIT) + + Copyright (c) 2015 Jacques Menuet + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#pragma once + +#include "RPMSerialInterface.h" + +#define WIN32_LEAN_AND_MEAN +#define NOMINMAX +#include + +class RPMSerialInterfaceWindows : public RPMSerialInterface +{ +public: + // Creates a Windows SerialInterface. + // The port name corresponds to a file name as used by the CreateFile system function. + // Such name can have various syntaxes, for example: + // - "COM4", + // - "\\\\.\\USBSER000" + // - "USB#VID_1FFB&PID_0089&MI_04#6&3ad40bf600004#" + RPMSerialInterfaceWindows(const std::string &portName, unsigned int baudRate, std::string *errorMessage = NULL); + + virtual ~RPMSerialInterfaceWindows(); + + virtual bool isOpen() const; + +private: + static HANDLE openPort(const std::string &portName, unsigned int baudRate, std::string *errorMessage); + + virtual bool writeBytes(const unsigned char *data, unsigned int dataSizeInBytes); + virtual bool readBytes(unsigned char *data, unsigned int dataSizeInBytes); + + HANDLE mPortHandle; +}; diff --git a/3rdparty/pololu/src/RPMSerialInterface.cpp b/3rdparty/pololu/src/RPMSerialInterface.cpp new file mode 100644 index 0000000000..3f91357342 --- /dev/null +++ b/3rdparty/pololu/src/RPMSerialInterface.cpp @@ -0,0 +1,263 @@ +/* + The MIT License (MIT) (http://opensource.org/licenses/MIT) + + Copyright (c) 2015 Jacques Menuet + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#include "RPMSerialInterface.h" + +#ifdef _WIN32 +#include "RPMSerialInterfaceWindows.h" +#else +#include "RPMSerialInterfacePOSIX.h" +#endif + +RPMSerialInterface *RPMSerialInterface::createSerialInterface(const std::string &portName, unsigned int baudRate, std::string *errorMessage) +{ + RPMSerialInterface *serialInterface = NULL; +#ifdef _WIN32 + serialInterface = new RPMSerialInterfaceWindows(portName, baudRate, errorMessage); +#else + serialInterface = new RPMSerialInterfacePOSIX(portName, errorMessage); + (void)baudRate; +#endif + + // If the interface couldn't be open properly, delete it + if (!serialInterface->isOpen()) { + delete serialInterface; + serialInterface = NULL; + } + + return serialInterface; +} + +RPMSerialInterface::RPMSerialInterface() + : mErrorMessage() +{ } + +RPMSerialInterface::~RPMSerialInterface() +{ } + +void RPMSerialInterface::clearErrorMessage() +{ + mErrorMessage.clear(); +} + +void RPMSerialInterface::setErrorMessage(const std::string &message) +{ + mErrorMessage = message; +} + +bool RPMSerialInterface::setTargetCP(unsigned char channelNumber, unsigned short target) +{ + clearErrorMessage(); + if (targetgetMaxChannelValue()) + return false; + + unsigned char command[4] = { 0x84, channelNumber, static_cast(target & 0x7F), static_cast((target >> 7) & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::setTargetPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned short target) +{ + clearErrorMessage(); + if (targetgetMaxChannelValue()) + return false; + unsigned char command[6] = { 0xAA, deviceNumber, static_cast(0x84 & 0x7F), channelNumber, static_cast(target & 0x7F), static_cast((target >> 7) & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::setTargetMSSCP(unsigned char miniSCCChannelNumber, unsigned char normalizedTarget) +{ + clearErrorMessage(); + if (normalizedTarget>254) + return false; + unsigned char command[3] = { 0xFF, miniSCCChannelNumber, normalizedTarget }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::setSpeedCP(unsigned char channelNumber, unsigned short speed) +{ + clearErrorMessage(); + unsigned char command[4] = { 0x87, channelNumber, static_cast(speed & 0x7F), static_cast((speed >> 7) & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::setSpeedPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned short speed) +{ + clearErrorMessage(); + unsigned char command[6] = { 0xAA, deviceNumber, static_cast(0x87 & 0x7F), channelNumber, static_cast(speed & 0x7F), static_cast((speed >> 7) & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::setAccelerationCP(unsigned char channelNumber, unsigned char acceleration) +{ + clearErrorMessage(); + unsigned short accelerationAsShort = acceleration; + unsigned char command[4] = { 0x89, channelNumber, static_cast(accelerationAsShort & 0x7F), static_cast((accelerationAsShort >> 7) & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::setAccelerationPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned char acceleration) +{ + clearErrorMessage(); + unsigned short accelerationAsShort = acceleration; + unsigned char command[6] = { 0xAA, deviceNumber, static_cast(0x89 & 0x7F), channelNumber, static_cast(accelerationAsShort & 0x7F), static_cast((accelerationAsShort >> 7) & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::getPositionCP(unsigned char channelNumber, unsigned short &position) +{ + clearErrorMessage(); + + position = 0; + + unsigned char command[2] = { 0x90, channelNumber }; + if (!writeBytes(command, sizeof(command))) + return false; + + unsigned char response[2] = { 0x00, 0x00 }; + if (!readBytes(response, sizeof(response))) + return false; + + position = response[0] + 256*response[1]; + return true; +} + +bool RPMSerialInterface::getPositionPP(unsigned char deviceNumber, unsigned char channelNumber, unsigned short &position) +{ + clearErrorMessage(); + + position = 0; + + unsigned char command[4] = { 0xAA, deviceNumber, static_cast(0x90 & 0x7F), channelNumber }; + if (!writeBytes(command, sizeof(command))) + return false; + + unsigned char response[2] = { 0x00, 0x00 }; + if (!readBytes(response, sizeof(response))) + return false; + + position = response[0] + 256*response[1]; + return true; +} + +bool RPMSerialInterface::getMovingStateCP(bool &servosAreMoving) +{ + clearErrorMessage(); + + servosAreMoving = false; + unsigned char command = 0x93; + if (!writeBytes(&command, sizeof(command))) + return false; + + unsigned char response = 0x00; + if (!readBytes(&response, sizeof(response))) + return false; + + if (response!=0x00 && response!=0x01) + return false; + + servosAreMoving = (response==0x01); + return true; +} + +bool RPMSerialInterface::getMovingStatePP(unsigned char deviceNumber, bool &servosAreMoving) +{ + clearErrorMessage(); + + servosAreMoving = false; + unsigned char command[3] = { 0xAA, deviceNumber, static_cast(0x93 & 0x7F) }; + if (!writeBytes(command, sizeof(command))) + return false; + + unsigned char response = 0x00; + if (!readBytes(&response, sizeof(response))) + return false; + + servosAreMoving = (response==0x01); + return true; +} + +bool RPMSerialInterface::getErrorsCP(unsigned short &errors) +{ + clearErrorMessage(); + + unsigned char command = 0xA1; + if (!writeBytes(&command, sizeof(command))) + return false; + + unsigned char response[2] = { 0x00, 0x00 }; + if (!readBytes(response, sizeof(response))) + return false; + + errors = (response[0] & 0x7F) + 256 * (response[1] & 0x7F); // Need to check this code on real errors! + return true; +} + +bool RPMSerialInterface::getErrorsPP(unsigned char deviceNumber, unsigned short &errors) +{ + clearErrorMessage(); + + unsigned char command[3] = { 0xAA, deviceNumber, 0xA1 & 0x7F }; + if (!writeBytes(command, sizeof(command))) + return false; + + unsigned char response[2] = { 0x00, 0x00 }; + if (!readBytes(response, sizeof(response))) + return false; + + errors = (response[0] & 0x7F) + 256 * (response[1] & 0x7F); // Need to check this code on real errors! + return true; +} + +bool RPMSerialInterface::goHomeCP() +{ + clearErrorMessage(); + + unsigned char command = 0xA2; + if (!writeBytes(&command, sizeof(command))) + return false; + return true; +} + +bool RPMSerialInterface::goHomePP(unsigned char deviceNumber) +{ + clearErrorMessage(); + + unsigned char command[3] = { 0xAA, deviceNumber, 0xA2 & 0x7F }; + if (!writeBytes(command, sizeof(command))) + return false; + return true; +} diff --git a/3rdparty/pololu/src/RPMSerialInterfacePOSIX.cpp b/3rdparty/pololu/src/RPMSerialInterfacePOSIX.cpp new file mode 100644 index 0000000000..8a2941a4ed --- /dev/null +++ b/3rdparty/pololu/src/RPMSerialInterfacePOSIX.cpp @@ -0,0 +1,131 @@ +/* + The MIT License (MIT) (http://opensource.org/licenses/MIT) + + Copyright (c) 2015 Jacques Menuet + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#include "RPMSerialInterfacePOSIX.h" + +#include +#include +#include +#include + +#ifdef _WIN32 +#define O_NOCTTY 0 +#else +#include +#endif + +#include + +RPMSerialInterfacePOSIX::RPMSerialInterfacePOSIX(const std::string &portName, std::string *errorMessage) + : RPMSerialInterface(), + mFileDescriptor(-1) +{ + mFileDescriptor = openPort(portName, errorMessage); +} + +RPMSerialInterfacePOSIX::~RPMSerialInterfacePOSIX() +{ + if (isOpen()) { + // Before destroying the interface, we "go home" + goHomeCP(); + + close(mFileDescriptor); + } + mFileDescriptor = -1; +} + +bool RPMSerialInterfacePOSIX::isOpen() const +{ + return mFileDescriptor!=-1; +} + +int RPMSerialInterfacePOSIX::openPort(const std::string &portName, std::string *errorMessage) +{ + int fd = open(portName.c_str(), O_RDWR | O_NOCTTY); + if (fd == -1) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + stream << strerror(errno); + *errorMessage = stream.str(); + } + return -1; + } + +#ifndef _WIN32 + struct termios options; + tcgetattr(fd, &options); + options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + options.c_oflag &= ~(ONLCR | OCRNL); + tcsetattr(fd, TCSANOW, &options); +#endif + + return fd; +} + +bool RPMSerialInterfacePOSIX::writeBytes(const unsigned char *data, unsigned int numBytesToWrite) +{ + if (!isOpen()) + return false; + + // See http://linux.die.net/man/2/write + ssize_t ret = write(mFileDescriptor, data, numBytesToWrite); + if (ret==-1) { + std::stringstream stream; + stream << "Unable to write bytes to serial port. "; + stream << "Error code " << errno; + setErrorMessage(stream.str()); + return false; + } + else if (ret!=numBytesToWrite) { + std::stringstream stream; + stream << "Unable to write bytes to serial port. Wrote only " << ret << " out of " << numBytesToWrite; + setErrorMessage(stream.str()); + return false; + } + + return true; +} + +bool RPMSerialInterfacePOSIX::readBytes(unsigned char *data, unsigned int numBytesToRead) +{ + if (!isOpen()) + return false; + + // See http://linux.die.net/man/2/read + ssize_t ret = read(mFileDescriptor, data, numBytesToRead); + if (ret==-1) { + std::stringstream stream; + stream << "Unable to read bytes from serial port. "; + stream << "Error code " << errno; + setErrorMessage(stream.str()); + return false; + } + else if (ret!=numBytesToRead) { + std::stringstream stream; + stream << "Unable to read bytes from serial port. Read only " << ret << " out of " << numBytesToRead; + setErrorMessage(stream.str()); + return false; + } + return true; +} diff --git a/3rdparty/pololu/src/RPMSerialInterfaceWindows.cpp b/3rdparty/pololu/src/RPMSerialInterfaceWindows.cpp new file mode 100644 index 0000000000..dfa550c665 --- /dev/null +++ b/3rdparty/pololu/src/RPMSerialInterfaceWindows.cpp @@ -0,0 +1,193 @@ +/* + The MIT License (MIT) (http://opensource.org/licenses/MIT) + + Copyright (c) 2015 Jacques Menuet + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#include "RPMSerialInterfaceWindows.h" + +#include + +RPMSerialInterfaceWindows::RPMSerialInterfaceWindows(const std::string &portName, unsigned int baudRate, std::string *errorMessage) + : RPMSerialInterface(), + mPortHandle(NULL) +{ + mPortHandle = openPort(portName, baudRate, errorMessage); +} + +RPMSerialInterfaceWindows::~RPMSerialInterfaceWindows() +{ + if (isOpen()) { + // Before destroying the interface, we "go home" + goHomeCP(); + + CloseHandle(mPortHandle); + } + mPortHandle = NULL; +} + +bool RPMSerialInterfaceWindows::isOpen() const +{ + return mPortHandle!=INVALID_HANDLE_VALUE; +} + +// Open the port at given baud rate. Return the port handle when successful, otherwise return INVALID_HANDLE_VALUE +HANDLE RPMSerialInterfaceWindows::openPort(const std::string &portName, unsigned int baudRate, std::string *errorMessage) +{ + // Open the serial port + HANDLE port = CreateFileA(portName.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); + if (port == INVALID_HANDLE_VALUE) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + DWORD lastError = GetLastError(); + switch (lastError) { + case ERROR_ACCESS_DENIED: stream << "Access denied. The device might be used by another program"; + break; + case ERROR_FILE_NOT_FOUND: stream << "File not found. The port name might be wrong"; + break; + default: + stream << std::hex << "Error code 0x" << lastError; // To test! + break; + } + *errorMessage = stream.str(); + } + return INVALID_HANDLE_VALUE; + } + + // Set the timeouts + COMMTIMEOUTS timeouts; + BOOL success = GetCommTimeouts(port, &timeouts); + if (!success) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + stream << "Unable to get Comm Timeouts. " << std::hex << "Error code 0x" << GetLastError(); + *errorMessage = stream.str(); + } + CloseHandle(port); + return INVALID_HANDLE_VALUE; + } + + timeouts.ReadIntervalTimeout = 1000; + timeouts.ReadTotalTimeoutConstant = 1000; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 1000; + timeouts.WriteTotalTimeoutMultiplier = 0; + success = SetCommTimeouts(port, &timeouts); + if (!success) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + stream << "Unable to set Comm Timeouts. " << std::hex << "Error code 0x" << GetLastError(); + *errorMessage = stream.str(); + } + CloseHandle(port); + return INVALID_HANDLE_VALUE; + } + + // Set the baud rate + DCB commState; + success = GetCommState(port, &commState); + if (!success) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + stream << "Unable to get Comm State. " << std::hex << "Error code 0x" << GetLastError(); + *errorMessage = stream.str(); + } + CloseHandle(port); + return INVALID_HANDLE_VALUE; + } + + commState.BaudRate = baudRate; + success = SetCommState(port, &commState); + if (!success) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + stream << "Unable to set Comm State. " << std::hex << "Error code 0x" << GetLastError(); + *errorMessage = stream.str(); + } + CloseHandle(port); + return INVALID_HANDLE_VALUE; + } + + // Flush out any bytes received from the device earlier + success = FlushFileBuffers(port); + if (!success) { + if (errorMessage) { + std::stringstream stream; + stream << "Failed to open serial port \"" << portName << "\". "; + stream << "Unable to flush port buffers. " << std::hex << "Error code 0x" << GetLastError(); + *errorMessage = stream.str(); + } + CloseHandle(port); + return INVALID_HANDLE_VALUE; + } + + return port; +} + +bool RPMSerialInterfaceWindows::writeBytes(const unsigned char *data, unsigned int numBytesToWrite) +{ + if (!isOpen()) + return false; + + DWORD bytesTransferred = 0; + BOOL success = WriteFile(mPortHandle, data, numBytesToWrite, &bytesTransferred, NULL); + if (!success) { + std::stringstream stream; + stream << "Unable to write bytes to serial port. " << std::hex << "Error code 0x" << GetLastError(); + setErrorMessage(stream.str()); + return false; + } + + if (numBytesToWrite!=bytesTransferred) { + std::stringstream stream; + stream << "Unable to write bytes to serial port. Wrote only " << bytesTransferred << " out of " << numBytesToWrite; + setErrorMessage(stream.str()); + return false; + } + return true; +} + +bool RPMSerialInterfaceWindows::readBytes(unsigned char *data, unsigned int numBytesToRead) +{ + if (!isOpen()) + return false; + + DWORD bytesTransferred = 0; + BOOL success = ReadFile(mPortHandle, data, numBytesToRead, &bytesTransferred, NULL); + if (!success) { + std::stringstream stream; + stream << "Unable to read bytes from serial port. " << std::hex << "Error code 0x" << GetLastError(); + setErrorMessage(stream.str()); + return false; + } + + if (numBytesToRead!=bytesTransferred) { + std::stringstream stream; + stream << "Unable to read bytes from serial port. Read only " << bytesTransferred << " out of " << numBytesToRead; + setErrorMessage(stream.str()); + return false; + } + return true; +} diff --git a/CMakeLists.txt b/CMakeLists.txt index b15c567720..452ab4c4aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -520,7 +520,7 @@ VP_OPTION(USE_PYLON Pylon "" "Include Pylon SDK support for Basle VP_OPTION(USE_UEYE Ueye "" "Include uEye SDK support for IDS cameras" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_COMEDI Comedi "" "Include comedi (linux control and measurement device interface) support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_FTIITSDK FTIITSDK "" "Include IIT force-torque SDK support" "" ON IF NOT WINRT AND NOT IOS) -VP_OPTION(USE_BICLOPS BICLOPS "" "Include Biclops support" "" ON IF NOT WINRT AND NOT IOS) +VP_OPTION(USE_BICLOPS BICLOPS "" "Include biclops support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_PTU46 PTU46 "" "Include ptu-46 support" "" ON IF UNIX AND NOT WINRT AND NOT IOS) VP_OPTION(USE_FLIRPTUSDK FlirPtuSDK "" "Include Flir PTU SDK support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_CMU1394 CMU1394 "" "Include cmu1494 support" "" ON IF WIN32 AND NOT WINRT AND NOT IOS) @@ -600,6 +600,16 @@ endif() # Upgrade c++ standard to 14 for pcl 1.9.1.99 that enables by default c++ 14 standard if(USE_PCL) + # PCL is used in modules gui, sensor and mbt. + # In these modules we cannot directly use PCL_INCLUDE_DIRS and PCL_LIBRARIES using: + # list(APPEND opt_incs ${PCL_INCLUDE_DIRS}) + # list(APPEND opt_libs ${PCL_LIBRARIES}) + # Using PCL_LIBRARIES works to build visp libraries, embedded examples, demos, tests and tutorials thanks to the + # components. But when examples, demos, tests and tutorials are build outside ViSP workspace as independent projects + # that are using ViSP as 3rd-party we lead to build issues due to VTK headers and libraries that are not found. + # That's why here, we are using vp_find_pcl() macro that will set PCL_DEPS_INCLUDE_DIRS and PCL_DEPS_LIBRARIES + # that contains also VTK material location. + vp_find_pcl(PCL_LIBRARIES PCL_DEPS_INCLUDE_DIRS PCL_DEPS_LIBRARIES) if(PCL_VERSION VERSION_GREATER 1.9.1) # pcl > 1.9.1 requires c++14 # if c++14 option is OFF, force to c++14 @@ -627,16 +637,17 @@ endif() # ---------------------------------------------------------------------------- # Build-in 3rd parties. Should be after c++ standard potential modification # ---------------------------------------------------------------------------- -VP_OPTION(WITH_PTHREAD "" "" "Build pthread as built-in library" "" ON IF (NOT USE_THREADS) AND (WIN32 OR MINGW) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0)) +VP_OPTION(WITH_PTHREAD "" "" "Build pthread as built-in library" "" ON IF (NOT USE_THREADS) AND (WIN32 OR MINGW) AND (NOT WINRT)) # Since C99 is not supported by MSVC 2010 or prior, we disable apriltag if MSVC < 2012 -VP_OPTION(WITH_APRILTAG "" "" "Build AprilTag as built-in library" "" ON IF (USE_THREADS OR WITH_PTHREAD) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0) AND (NOT MSVC_VERSION LESS 1700)) +VP_OPTION(WITH_APRILTAG "" "" "Build AprilTag as built-in library" "" ON IF (USE_THREADS OR WITH_PTHREAD) AND (NOT WINRT) AND (NOT MSVC_VERSION LESS 1700)) VP_OPTION(WITH_APRILTAG_BIG_FAMILY "" "" "Build AprilTag big family (41h12, 48h12, 49h12, 52h13)" "" OFF IF WITH_APRILTAG) VP_OPTION(WITH_ATIDAQ "" "" "Build atidaq-c as built-in library" "" ON IF USE_COMEDI AND NOT WINRT) VP_OPTION(WITH_CLIPPER "" "" "Build clipper as built-in library" "" ON IF USE_OPENCV) VP_OPTION(WITH_LAPACK "" "" "Build lapack as built-in library" "" ON IF NOT USE_LAPACK) -VP_OPTION(WITH_QBDEVICE "" "" "Build qbdevice-api as built-in library" "" ON IF (NOT WINRT) AND (NOT IOS)) -VP_OPTION(WITH_TAKKTILE2 "" "" "Build Right Hand takktile2 driver as built-in library" "" ON IF (NOT WIN32) AND (NOT WINRT) AND (NOT IOS) AND (NOT ANDROID)) -VP_OPTION(WITH_CATCH2 "" "" "Use catch2 for testing" "" ON) +VP_OPTION(WITH_QBDEVICE "" "" "Build qbdevice-api as built-in library" "" ON IF (VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98) AND (NOT WINRT) AND (NOT IOS)) +VP_OPTION(WITH_TAKKTILE2 "" "" "Build Right Hand takktile2 driver as built-in library" "" ON IF (VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98) AND (NOT WIN32) AND (NOT WINRT) AND (NOT IOS) AND (NOT ANDROID)) +VP_OPTION(WITH_CATCH2 "" "" "Use catch2" "" ON IF (VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98)) +VP_OPTION(WITH_POLOLU "" "" "Build rapa pololu as built-in library" "" ON IF (NOT WINRT) AND (NOT IOS) AND (NOT ANDROID)) # ---------------------------------------------------------------------------- # Check for specific functions. Should be after cxx standard detection in VISPDetectCXXStandard.cmake and potential modification depending on pcl, realsense2, libfranka @@ -885,7 +896,7 @@ VP_SET(VISP_HAVE_UR_RTDE TRUE IF (BUILD_MODULE_visp_robot AND USE_UR_RTDE)) VP_SET(VISP_HAVE_FRANKA TRUE IF (BUILD_MODULE_visp_robot AND USE_FRANKA)) VP_SET(VISP_HAVE_JACOSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_JACOSDK)) VP_SET(VISP_HAVE_MAVSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_MAVSDK)) -VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS AND USE_THREADS)) +VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS)) VP_SET(VISP_HAVE_PTU46 TRUE IF (BUILD_MODULE_visp_robot AND USE_PTU46)) VP_SET(VISP_HAVE_FLIR_PTU_SDK TRUE IF (BUILD_MODULE_visp_robot AND USE_FLIRPTUSDK)) VP_SET(VISP_HAVE_ARSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_ARSDK)) @@ -928,6 +939,7 @@ VP_SET(VISP_HAVE_APRILTAG TRUE IF (BUILD_MODULE_visp_detection AND WITH_APRIL VP_SET(VISP_HAVE_APRILTAG_BIG_FAMILY TRUE IF (BUILD_MODULE_visp_detection AND WITH_APRILTAG AND WITH_APRILTAG_BIG_FAMILY)) VP_SET(VISP_HAVE_QBDEVICE TRUE IF (BUILD_MODULE_visp_robot AND WITH_QBDEVICE)) VP_SET(VISP_HAVE_TAKKTILE2 TRUE IF (BUILD_MODULE_visp_robot AND WITH_TAKKTILE2)) +VP_SET(VISP_HAVE_POLOLU TRUE IF (BUILD_MODULE_visp_robot AND WITH_POLOLU)) VP_SET(VISP_HAVE_CATCH2 TRUE IF (BUILD_MODULE_visp_core AND WITH_CATCH2)) VP_SET(VISP_HAVE_QUALISYS TRUE IF (BUILD_MODULE_visp_sensor AND USE_QUALISYS)) @@ -1511,6 +1523,7 @@ status(" \\-Use ffmpeg:" USE_FFMPEG THEN "yes" ELSE "no") status(" Use Virtuose:" USE_VIRTUOSE THEN "yes" ELSE "no") status(" Use qbdevice (built-in):" WITH_QBDEVICE THEN "yes (ver ${QBDEVICE_VERSION})" ELSE "no") status(" Use takktile2 (built-in):" WITH_TAKKTILE2 THEN "yes (ver ${TAKKTILE2_VERSION})" ELSE "no") +status(" Use pololu (built-in):" WITH_POLOLU THEN "yes (ver ${POLOLU_VERSION})" ELSE "no") status("") status(" GUI: ") status(" Use X11:" USE_X11 THEN "yes" ELSE "no") diff --git a/ChangeLog.txt b/ChangeLog.txt index 55ca45d97c..31ad7472a8 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -7,24 +7,37 @@ ViSP 3.x.x (Version in development) - Contributors: - . Fabien Spindler, Florent Lamiraux + . Fabien Spindler, Romain Lagneau, Pierre Perraud, Florent Lamiraux, Samuel Felton, François Chaumette, + Souriya Trinh - New classes - . - - Deprecated removed - . vpPlanarObjectDetector, vpFernClassifier classes are removed + . vpPololu and vpRobotPololuPtu to control respectively a servo motor using a pololu maestro board or a 3D printed + 2 dof pan-tilt unit. Visual servoing example provided in example/servo-pololu-ptu. Tests available in + modules/robot/test/servo-pololu/ + - Deprecated + . vpPlanarObjectDetector, vpFernClassifier deprecated classes are removed . End of supporting c++98 standard. As a consequence, ViSP is no more compatible with Ubuntu 12.04 + . vpDisplay::displayCharString() is marked deprecated. Use vpDisplay::displayText() instead - New features and improvements . Introduce applications in apps folder, a collection of useful tools that have a dependency to the install target . Bump minimal c++ standard to c++11 + . Speed up build by including only opencv2/opencv_modules.hpp instead of opencv2/opencv.hpp header in vpConfig.h - Applications . Migrate eye-to-hand tutorials in apps - Tutorials . Update tutorial: How to use Blender to generate simulated data for model-based tracking experiments with step by step detailed explanations https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-tracking-mb-generic-rgbd-Blender.html + . New tutorial: Installation from prebuilt Conda packages for Linux / OSX / Windows + https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-install-conda-package.html - Bug fixed . [#1251] Bug in vpDisplay::displayFrame() + . [#1270] Build issue around std::clamp and optional header which are not found with cxx17 + standard enabled + . [#1272] Unable to build ViSP with PCL/VTK build from source, VTK headers are not found by ViSP + . [#1273] Build error in visp_java without deprecated functionalities + . [#1274] Build issue no member named clamp in namespace std + . [#1279] Issue in vpPoseVector json serialization ---------------------------------------------- ViSP 3.6.0 (released September 22, 2023) - Contributors: diff --git a/cmake/PCLTools.cmake b/cmake/PCLTools.cmake index 067620172b..ee1099c80a 100644 --- a/cmake/PCLTools.cmake +++ b/cmake/PCLTools.cmake @@ -33,10 +33,40 @@ # ############################################################################# +# Remove BUILD_INTERFACE from __include_dirs +# IN/OUT: __include_dirs +# +# If __include_dirs contains "$" as input, +# it will be filtered as output to /home/VTK/install/include/vtk-9.3 +macro(vp_filter_build_interface __include_dirs) + if(${__include_dirs}) + set(__include_dirs_filtered) + foreach(inc_ ${${__include_dirs}}) + string(REGEX REPLACE "\\$" "" inc_ ${inc_}) + list(APPEND __include_dirs_filtered ${inc_}) + endforeach() + + set(${__include_dirs} ${__include_dirs_filtered}) + endif() +endmacro() + # Find pcl libraries and dependencies # IN: pcl_libraries # OUT: pcl_deps_include_dirs # OUT: pcl_deps_libraries +# +# PCL_LIBRARIES contains VTK 3rd party such as vtkalglib and not /usr/local/Cellar/vtk/6.3.0/lib/libvtkalglib-6.3.1.dylib +# full path as requested to use ViSP as 3rd party. This is the case for all VTK libraries that are PCL dependencies. +# The build of ViSP works with PCL_LIBRARIES since in that case thanks to vtkalglib properties, CMake +# is able to find the real name and location of the libraries. +# But when ViSP is used as a 3rd party where it should import PCL libraries, it doesn't work with +# PCL_LIBRARIES and especially with VTK_LIBRARIES. +# The solution here is to get the full location of VTK_LIBRARIES libraries thanks to the properties and link +# with these names. +# An other way could be to include PCLConfig.cmake, but in that case, visp-config and visp.pc +# will be not able to give the names of PCL libraries when used without CMake. +# macro(vp_find_pcl pcl_libraries pcl_deps_include_dirs pcl_deps_libraries) foreach(lib_ ${${pcl_libraries}}) mark_as_advanced(${lib_}_LOCATION) @@ -97,6 +127,10 @@ macro(vp_find_pcl pcl_libraries pcl_deps_include_dirs pcl_deps_libraries) endforeach() vp_list_unique(PCL_VTK_IMPORTED_LIBS) vp_list_unique(PCL_VTK_IMPORTED_INCS) + + # Filter "$" into /home/VTK/install/include/vtk-9.3 + vp_filter_build_interface(PCL_VTK_IMPORTED_INCS) + list(APPEND ${pcl_deps_include_dirs} ${PCL_VTK_IMPORTED_INCS}) # Filter "\$;\$;\$" into "vtkCommonMath;opengl32;glu32" @@ -159,6 +193,10 @@ macro(vp_find_pcl pcl_libraries pcl_deps_include_dirs pcl_deps_libraries) endforeach() vp_list_unique(PCL_VTK_IMPORTED_LIBS) vp_list_unique(PCL_VTK_IMPORTED_INCS) + + # Filter "$" into /home/VTK/install/include/vtk-9.3 + vp_filter_build_interface(PCL_VTK_IMPORTED_INCS) + list(APPEND ${pcl_deps_include_dirs} ${PCL_VTK_IMPORTED_INCS}) while(PCL_VTK_IMPORTED_LIBS) diff --git a/cmake/VISP3rdParty.cmake b/cmake/VISP3rdParty.cmake index eb6bc33a4b..65426c7b36 100644 --- a/cmake/VISP3rdParty.cmake +++ b/cmake/VISP3rdParty.cmake @@ -88,3 +88,11 @@ if(WITH_CATCH2) set(CATCH2_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/catch2") set(CATCH2_VERSION ${CATCH2_MAJOR_VERSION}.${CATCH2_MINOR_VERSION}.${CATCH2_PATCH_VERSION}) endif() + +if(WITH_POLOLU) + set(POLOLU_LIBRARY visp_pololu) + add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/pololu") + set(POLOLU_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/pololu/include") + set(POLOLU_LIBRARIES ${POLOLU_LIBRARY}) + set(POLOLU_VERSION ${POLOLU_MAJOR_VERSION}.${POLOLU_MINOR_VERSION}.${POLOLU_PATCH_VERSION}) +endif() diff --git a/cmake/VISPDetectCXXStandard.cmake b/cmake/VISPDetectCXXStandard.cmake index ab040d821c..6169763899 100644 --- a/cmake/VISPDetectCXXStandard.cmake +++ b/cmake/VISPDetectCXXStandard.cmake @@ -8,12 +8,27 @@ if(DEFINED USE_CXX_STANDARD) if(USE_CXX_STANDARD STREQUAL "11") set(CMAKE_CXX_STANDARD 11) set(VISP_CXX_STANDARD ${VISP_CXX_STANDARD_11}) + vp_check_compiler_flag(CXX "-std=c++11" HAVE_STD_CXX11_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx11.cpp") + if(HAVE_STD_CXX11_FLAG) + set(CXX11_CXX_FLAGS "-std=c++11" CACHE STRING "C++ compiler flags for C++11 support") + mark_as_advanced(HAVE_STD_CXX11_FLAG) + endif() elseif(USE_CXX_STANDARD STREQUAL "14") set(CMAKE_CXX_STANDARD 14) set(VISP_CXX_STANDARD ${VISP_CXX_STANDARD_14}) + vp_check_compiler_flag(CXX "-std=c++14" HAVE_STD_CXX14_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx14.cpp") + if(HAVE_STD_CXX14_FLAG) + set(CXX14_CXX_FLAGS "-std=c++14" CACHE STRING "C++ compiler flags for C++14 support") + mark_as_advanced(HAVE_STD_CXX14_FLAG) + endif() elseif(USE_CXX_STANDARD STREQUAL "17") set(CMAKE_CXX_STANDARD 17) set(VISP_CXX_STANDARD ${VISP_CXX_STANDARD_17}) + vp_check_compiler_flag(CXX "-std=c++17" HAVE_STD_CXX17_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx17.cpp") + if(HAVE_STD_CXX17_FLAG) + set(CXX17_CXX_FLAGS "-std=c++17" CACHE STRING "C++ compiler flags for C++17 support") + mark_as_advanced(HAVE_STD_CXX17_FLAG) + endif() endif() set(CMAKE_CXX_EXTENSIONS OFF) # use -std=c++11 instead of -std=gnu++11 @@ -30,19 +45,43 @@ else() # That's why we check more in depth for cxx_override that is not available with g++ 4.6.3 list (FIND CMAKE_CXX11_COMPILE_FEATURES "cxx_override" _index) if (${_index} GREATER -1) - set(CXX11_STANDARD_FOUND ON CACHE STRING "cxx11 standard") - mark_as_advanced(CXX11_STANDARD_FOUND) + # Setting CMAKE_CXX_STANDARD doesn't affect check_cxx_source_compiles() used to detect isnan() in FindIsNaN.cmake + # or erfc() in FindErfc.cmake. + # That's why we have the following lines that are used to set cxx flags corresponding to the c++ standard + vp_check_compiler_flag(CXX "-std=c++11" HAVE_STD_CXX11_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx11.cpp") + if(HAVE_STD_CXX11_FLAG) + set(CXX11_CXX_FLAGS "-std=c++11" CACHE STRING "C++ compiler flags for C++11 support") + set(CXX11_STANDARD_FOUND ON CACHE STRING "cxx11 standard") + mark_as_advanced(CXX11_STANDARD_FOUND) + mark_as_advanced(CXX11_CXX_FLAGS) + mark_as_advanced(HAVE_STD_CXX11_FLAG) + endif() endif() endif() if(CMAKE_CXX14_COMPILE_FEATURES) - set(CXX14_STANDARD_FOUND ON CACHE STRING "cxx14 standard") - mark_as_advanced(CXX14_STANDARD_FOUND) + # Additionnal check in case of c++14 is incomplete and also needed to set CXX14_CXX_FLAGS + vp_check_compiler_flag(CXX "-std=c++14" HAVE_STD_CXX14_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx14.cpp") + if(HAVE_STD_CXX14_FLAG) + set(CXX14_CXX_FLAGS "-std=c++14" CACHE STRING "C++ compiler flags for C++14 support") + set(CXX14_STANDARD_FOUND ON CACHE STRING "cxx14 standard") + mark_as_advanced(CXX14_STANDARD_FOUND) + mark_as_advanced(CXX14_CXX_FLAGS) + mark_as_advanced(HAVE_STD_CXX14_FLAG) + endif() endif() if(CMAKE_CXX17_COMPILE_FEATURES) - set(CXX17_STANDARD_FOUND ON CACHE STRING "cxx17 standard") - mark_as_advanced(CXX17_STANDARD_FOUND) + # c++17 seems available, but on Fedora 25 and g++ 6.3.1 it seems incomplete where + # optional header is not found as well as std::clamp() + vp_check_compiler_flag(CXX "-std=c++17" HAVE_STD_CXX17_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx17.cpp") + if(HAVE_STD_CXX17_FLAG) + set(CXX17_CXX_FLAGS "-std=c++17" CACHE STRING "C++ compiler flags for C++17 support") + set(CXX17_STANDARD_FOUND ON CACHE STRING "cxx17 standard") + mark_as_advanced(CXX17_STANDARD_FOUND) + mark_as_advanced(CXX17_CXX_FLAGS) + mark_as_advanced(HAVE_STD_CXX17_FLAG) + endif() endif() # Set default c++ standard to 17, the first in the list @@ -83,26 +122,4 @@ else() set(VISP_CXX_STANDARD ${VISP_CXX_STANDARD_17}) endif() endif() - -endif() - -# Setting CMAKE_CXX_STANDARD doesn't affect check_cxx_source_compiles() used to detect isnan() in FindIsNaN.cmake -# or erfc() in FindErfc.cmake. -# That's why we have the following lines that are used to set cxx flags corresponding to the c++ standard -vp_check_compiler_flag(CXX "-std=c++11" HAVE_STD_CXX11_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx11.cpp") -if(HAVE_STD_CXX11_FLAG) - set(CXX11_CXX_FLAGS "-std=c++11" CACHE STRING "C++ compiler flags for C++11 support") - mark_as_advanced(CXX11_CXX_FLAGS) -endif() - -vp_check_compiler_flag(CXX "-std=c++14" HAVE_STD_CXX14_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx14.cpp") -if(HAVE_STD_CXX14_FLAG) - set(CXX14_CXX_FLAGS "-std=c++14" CACHE STRING "C++ compiler flags for C++14 support") - mark_as_advanced(CXX14_CXX_FLAGS) -endif() - -vp_check_compiler_flag(CXX "-std=c++17" HAVE_STD_CXX17_FLAG "${PROJECT_SOURCE_DIR}/cmake/checks/cxx17.cpp") -if(HAVE_STD_CXX17_FLAG) - set(CXX17_CXX_FLAGS "-std=c++17" CACHE STRING "C++ compiler flags for C++17 support") - mark_as_advanced(CXX17_CXX_FLAGS) endif() diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in index ebdd634d43..5713458ae2 100644 --- a/cmake/templates/vpConfig.h.in +++ b/cmake/templates/vpConfig.h.in @@ -40,33 +40,33 @@ #include #if defined _MSC_VER && _MSC_VER >= 1200 - #pragma warning( disable: 4100 4127 4251 4275 4351 4514 4668 4710 4820 ) - #if _MSC_VER >= 1400 // 1400 = MSVC 8 2005 - #pragma warning( disable: 4548 ) - #endif - #if _MSC_VER > 1500 // 1500 = MSVC 9 2008 - #pragma warning( disable: 4986 ) - #endif - #ifdef WINRT - #pragma warning(disable:4447) - #endif - - // 4100 : undocumented ("unreferenced formal parameter") - // 4127 : conditional expression is constant - // 4251 : 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2', ie. disable warnings related to inline functions - // 4275 : non – DLL-interface classkey 'identifier' used as base for DLL-interface classkey 'identifier' - // 4351 : new behavior: elements of array will be default initialized - // 4447 : Disable warning 'main' signature found without threading model - // 4514 : 'function' : unreferenced inline function has been removed - // 4548 : expression before comma has no effect - // 4668 : 'symbol' is not defined as a preprocessor macro, replacing with '0' for 'directives' - // 4710 : 'function' : function not inlined - // 4820 : 'bytes' bytes padding added after construct 'member_name' - // 4986 : undocumented - - #ifndef NOMINMAX - #define NOMINMAX - #endif +#pragma warning( disable: 4100 4127 4251 4275 4351 4514 4668 4710 4820 ) +#if _MSC_VER >= 1400 // 1400 = MSVC 8 2005 +#pragma warning( disable: 4548 ) +#endif +#if _MSC_VER > 1500 // 1500 = MSVC 9 2008 +#pragma warning( disable: 4986 ) +#endif +#ifdef WINRT +#pragma warning(disable:4447) +#endif + +// 4100 : undocumented ("unreferenced formal parameter") +// 4127 : conditional expression is constant +// 4251 : 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2', ie. disable warnings related to inline functions +// 4275 : non – DLL-interface classkey 'identifier' used as base for DLL-interface classkey 'identifier' +// 4351 : new behavior: elements of array will be default initialized +// 4447 : Disable warning 'main' signature found without threading model +// 4514 : 'function' : unreferenced inline function has been removed +// 4548 : expression before comma has no effect +// 4668 : 'symbol' is not defined as a preprocessor macro, replacing with '0' for 'directives' +// 4710 : 'function' : function not inlined +// 4820 : 'bytes' bytes padding added after construct 'member_name' +// 4986 : undocumented + +#ifndef NOMINMAX +#define NOMINMAX +#endif #endif #if defined _MSC_VER && (_MSC_VER == 1500) @@ -104,7 +104,7 @@ #define VISP_VERSION_PATCH ${VISP_VERSION_PATCH} // ViSP version with dots "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}". -#cmakedefine VISP_VERSION ${VISP_VERSION} +#cmakedefine VISP_VERSION $ { VISP_VERSION } // ViSP version as an integer #define VP_VERSION_INT(a, b, c) (a<<16 | b<<8 | c) @@ -115,7 +115,7 @@ // Enable debug and trace printings #cmakedefine VP_TRACE #cmakedefine VP_DEBUG -#cmakedefine VP_DEBUG_MODE ${VP_DEBUG_MODE} +#cmakedefine VP_DEBUG_MODE $ { VP_DEBUG_MODE } // ViSP library is either compiled static or shared // Used to set declspec(import, export) in headers if required under Windows @@ -185,7 +185,7 @@ #cmakedefine VISP_HAVE_D3D9 // Defined if one of the display device is available -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(HAVE_OPENCV_HIGHGUI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) # define VISP_HAVE_DISPLAY #endif @@ -398,6 +398,9 @@ #cmakedefine VISP_HAVE_BICLOPS #cmakedefine VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION +// Defined if Rapa Pololu Maestro 3rd party library available. +#cmakedefine VISP_HAVE_POLOLU + // Defined if Irisa's Ptu-46 pan-tilt head available. #cmakedefine VISP_HAVE_PTU46 diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index fc2f3cc9fa..f55c416da7 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -2203,6 +2203,7 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ VISP_HAVE_PCL \ VISP_HAVE_PIONEER \ VISP_HAVE_PNG \ + VISP_HAVE_POLOLU \ VISP_HAVE_PTHREAD \ VISP_HAVE_PTU46 \ VISP_HAVE_PYLON \ diff --git a/doc/image/tutorial/package/conda_sysroot.svg b/doc/image/tutorial/package/conda_sysroot.svg new file mode 100644 index 0000000000..f6ed242adc --- /dev/null +++ b/doc/image/tutorial/package/conda_sysroot.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + $CONDA_PREFIX/ + bin/ + conda-meta/ Conda managed directory + include/ + lib/ + + + + + + + + diff --git a/doc/image/tutorial/package/conda_sysroot_sources.svg b/doc/image/tutorial/package/conda_sysroot_sources.svg new file mode 100644 index 0000000000..f1f82a6b77 --- /dev/null +++ b/doc/image/tutorial/package/conda_sysroot_sources.svg @@ -0,0 +1,183 @@ + + + + + + + + + + + + + + $CONDA_PREFIX/ + bin/ + conda-meta/ + include/ + lib/ + src/ Put your sources here + + + + + + + + + + + diff --git a/doc/tutorial/package/tutorial-install-conda-package.dox b/doc/tutorial/package/tutorial-install-conda-package.dox new file mode 100644 index 0000000000..3432dc0801 --- /dev/null +++ b/doc/tutorial/package/tutorial-install-conda-package.dox @@ -0,0 +1,742 @@ +/** + +\page tutorial-install-conda-package Tutorial: Installation from prebuilt Conda packages for Linux / OSX / Windows +\tableofcontents + +Conda is both a package manager and an environment manager. It provides a simple way to install packages inside an isolated environment, avoiding the +installation alongside system wide packages and letting you switch between different environment that may use different and/or +incompatible versions. More especially, it can easily handle different Python versions and installations. +Conda packages are not restricted to Python and can be written in any language, so it is well suited for C++ or C++ / Python packages. +Conda also provides a package dependency management mechanism and can detect incompatible dependencies, saving the user from breaking their environment. + +Since release 3.5.0, ViSP is available as a Conda package from conda-forge +for x86 64 bits architectures on Linux, OSX and Windows platforms. + +If you are already familiar with Conda and your Conda installation is configured with `conda-forge` as the default channel, +then you can skip the first sections and directly go to the \ref install_visp_conda_package. Otherwise, these steps will guide +you on how to install and configure Conda first. + +If you are new to Conda, this cheat sheet +might be also very useful. + +\section install_conda Installing Conda + +We recommend to install Conda from the installers provided by the conda-forge community, such as +Miniforge, which will be already configured as desired. + +Since release v23.10, Conda is automatically configured to use `libmamba-solver` by default to resolve dependencies. We highly recommand +the use of this solver as it dramastically improve the performances. +To check your current Conda version, just run: +\verbatim +$ conda info +\endverbatim +and check for the `conda version` line. +If your version is `23.10.0` or above, you have nothing to do, `libmamba-solver` is already set-up by default. +Otherwise, we recommand you to update your Conda version: +\verbatim +$ conda update -n base conda -c conda-forge +\endverbatim + +\section check_conda_install Checking your Conda installation + +The initial Conda configuration can differ depending on the installer and distribution you have used. If you +installed Conda using Miniforge distribution as in previous section, then you can skip this section +and directly go to \ref install_visp_conda_package. If you have an old Conda installation, used another installer or are +ensure about your Conda configuration, please proceed this section carefully. +First, you need to ensure that the conda-forge channel is enabled in your Conda installation +and has priority: +\verbatim +$ conda config --show channels +\endverbatim + +Check that conda-forge is present in the list and appears first: +\verbatim +channels: + - conda-forge + - defaults +\endverbatim + +If not present in the list, you may add it permanently to your channels list using: +\verbatim +$ conda config --add channels conda-forge +\endverbatim + +If the order in the list (channel priority) is reversed, just type re-add +the conda-forge channel using the previous command and you should get the desired output. + +We also strongly advise that you use the strict channel priority mode, as recommended by Conda + here: +\verbatim +$ conda config --show channel_priority +\endverbatim + +\note You can also directly access to your Conda configuration by editing the `~/.condarc` file. + +\section create_conda_env Optional: Creating a new Conda environment + +After a fresh installation, your shell should have been configured to support Conda. +The current active environment (default is `base`) should appear in your command prompt between parenthesis, e.g.: +\verbatim +(base)$ +\endverbatim +`base` is the default and root environment. Most of the time, you should avoid installing packages directly in this +environment. Instead, you should create a new environment for each desired context.This can be done by: +\verbatim +$ conda create -n my-visp-env +\endverbatim +where `my-visp-env` is the name you want to give to this environment. + +At this point, your environment `my-visp-env` is created, but not active. +You can activate your environment `my-visp-env` by: +\verbatim +$ conda activate my-visp-env +\endverbatim + +The active environment between parenthesis in your command prompt should have been updated to `my-visp-env`. +You can also check this by: +\verbatim +$ conda env list +\endverbatim +This command will list all your existing Conda environment, and your active environment will have its name suffixed +by an asterisk (*). + +You can deactivate the active environment by: +\verbatim +$ conda deactivate +\endverbatim +which should get you back to the environment from which your did your last activation (recall that environments stack). + +It might be interesting to note that a sysroot directory structure is created for each Conda environment, when the environment is created. +This is then the place where your libraries, headers and so will be put when installing Conda packages. +You can access to the active environment sysroot directory directly using the `CONDA_PREFIX` environment variable. +Depending on how much populated your sysroot already is, you should have a directory structure similar to: + +\image html conda_sysroot.svg Typical sysroot directory hierarchy for each Conda environment. You may have more or less directories, depending on your Conda environment. + +\section install_visp_conda_package Installing ViSP Conda package + +One line install: +\verbatim +$ conda install visp +\endverbatim + +\section good_practices_conda Some good practices with Conda + +- Minimize system-wide packages, install as much as possible from Conda in your dedicated environment. + +- Be careful when mixing different channels: prefer to restrict to conda-forge channel only. + +- Do not install in the `base` environment, excepted Conda global or configuration packages (e.g. `conda-build`) + +- Keep Conda up-to-date: +\verbatim +$ conda update -n base conda -c conda-forge +\endverbatim + +- If you develop C++ code with Conda, it is a good practice to keep your project sources associated to one Conda environment. +This ease keeping integrity between code and the environment and mixing with dependencies managers such vcpkg. +One way to do so is to put all the code associated to an environment inside the Conda environment hierarchy sysroot. +More precisely, you could have a `src` directory in every Conda environment sysroot like this: +\image html conda_sysroot_sources.svg One way to organize your sources associated to a given Conda environment. + +Another way to do so is to create the Conda environment in a dedicated directory (commonly named `envs/`) within your project source, +as recommanded here +in the official Conda documentation: +\verbatim +$ conda create --prefix /envs +$ conda activate /envs +\endverbatim + +\section conda_package_config Configuration used for the Conda package + +To get an idea of the third parties available in the ViSP Conda package, we provide below the contents of the file +ViSP-third-party.txt file for the Conda Linux-64, OSX-64 and Windows-64 packages. + +\subsection conda_package_config_linux_64 Configuration for the Linux-64 Conda package + +\verbatim +========================================================== +General configuration information for ViSP 3.6.0 + + Version control: unknown + + Platform: + Timestamp: 2023-10-23T17:58:44Z + Host: Linux 6.2.0-1014-azure x86_64 + CMake: 3.27.6 + CMake generator: Unix Makefiles + CMake build tool: $BUILD_PREFIX/bin/make + Configuration: Release + + System information: + Number of CPU logical cores: 2 + Number of CPU physical cores: 2 + Total physical memory (in MiB): 6921 + OS name: Linux + OS release: 6.2.0-1014-azure + OS version: #14~22.04.1-Ubuntu SMP Wed Sep 13 16:15:26 UTC 2023 + OS platform: x86_64 + CPU name: Unknown P6 family + Is the CPU 64-bit? yes + Does the CPU have FPU? yes + CPU optimization: SSE2 SSE3 SSSE3 + + C/C++: + Built as dynamic libs?: yes + C++ Compiler: $BUILD_PREFIX/bin/x86_64-conda-linux-gnu-c++ (ver 12.3.0) + C++ flags (Release): -fvisibility-inlines-hidden -fmessage-length=0 -march=nocona -mtune=haswell -ftree-vectorize -fPIC -fstack-protector-strong -fno-plt -O2 -ffunction-sections -pipe -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -Wall -Wextra -fopenmp -pthread -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG + C++ flags (Debug): -fvisibility-inlines-hidden -fmessage-length=0 -march=nocona -mtune=haswell -ftree-vectorize -fPIC -fstack-protector-strong -fno-plt -O2 -ffunction-sections -pipe -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -Wall -Wextra -fopenmp -pthread -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g + C Compiler: $BUILD_PREFIX/bin/x86_64-conda-linux-gnu-cc + C flags (Release): -march=nocona -mtune=haswell -ftree-vectorize -fPIC -fstack-protector-strong -fno-plt -O2 -ffunction-sections -pipe -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -Wall -Wextra -fopenmp -pthread -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG + C flags (Debug): -march=nocona -mtune=haswell -ftree-vectorize -fPIC -fstack-protector-strong -fno-plt -O2 -ffunction-sections -pipe -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -Wall -Wextra -fopenmp -pthread -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g + Linker flags (Release): -Wl,-O2 -Wl,--sort-common -Wl,--as-needed -Wl,-z,relro -Wl,-z,now -Wl,--disable-new-dtags -Wl,--gc-sections -Wl,--allow-shlib-undefined -Wl,-rpath,$PREFIX/lib -Wl,-rpath-link,$PREFIX/lib -L$PREFIX/lib + Linker flags (Debug): -Wl,-O2 -Wl,--sort-common -Wl,--as-needed -Wl,-z,relro -Wl,-z,now -Wl,--disable-new-dtags -Wl,--gc-sections -Wl,--allow-shlib-undefined -Wl,-rpath,$PREFIX/lib -Wl,-rpath-link,$PREFIX/lib -L$PREFIX/lib + Use cxx standard: 17 + + ViSP modules: + To be built: core gui imgproc io java_bindings_generator klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi + Disabled: - + Disabled by dependency: - + Unavailable: java + + Python (for build): $PREFIX/bin/python + + Java: + ant: no + JNI: no + + Build options: + Build deprecated: yes + Build with moment combine: no + + OpenCV: + Version: 4.8.1 + Modules: calib3d core dnn features2d flann gapi highgui imgcodecs imgproc ml objdetect photo stitching video videoio alphamat aruco bgsegm bioinspired ccalib cvv datasets dnn_objdetect dnn_superres dpm face freetype fuzzy hdf hfs img_hash intensity_transform line_descriptor mcc optflow phase_unwrapping plot quality rapid reg rgbd saliency shape stereo structured_light superres surface_matching text tracking videostab wechat_qrcode xfeatures2d ximgproc xobjdetect xphoto + OpenCV dir: $PREFIX/lib/cmake/opencv4 + + Mathematics: + Blas/Lapack: yes + \- Use MKL: yes + \- Use OpenBLAS: no + \- Use Atlas: no + \- Use Netlib: no + \- Use GSL: no + \- Use Lapack (built-in): no + Use Eigen3: yes (ver 3.4.0) + Use OpenCV: yes (ver 4.8.1) + + Simulator: + Ogre simulator: + \- Use Ogre3D: no + \- Use OIS: no + Coin simulator: + \- Use Coin3D: no + \- Use SoWin: no + \- Use SoXt: no + \- Use SoQt: no + \- Use Qt5: no + \- Use Qt4: no + \- Use Qt3: no + + Media I/O: + Use JPEG: yes (ver 80) + Use PNG: yes (ver 1.6.39) + \- Use ZLIB: yes (ver 1.2.13) + Use OpenCV: yes (ver 4.8.1) + Use stb_image (built-in): yes (ver 2.27.0) + Use TinyEXR (built-in): yes (ver 1.0.2) + + Real robots: + Use Afma4: no + Use Afma6: no + Use Franka: no + Use Viper650: no + Use Viper850: no + Use ur_rtde: no + Use Kinova Jaco: no + Use aria (Pioneer): no + Use PTU46: no + Use Biclops PTU: no + Use Flir PTU SDK: no + Use MAVSDK: no + Use Parrot ARSDK: no + \-Use ffmpeg: no + Use Virtuose: no + Use qbdevice (built-in): yes (ver 2.6.0) + Use takktile2 (built-in): yes (ver 1.0.0) + + GUI: + Use X11: yes + Use GTK: no + Use OpenCV: yes (ver 4.8.1) + Use GDI: no + Use Direct3D: no + + Cameras: + Use DC1394-2.x: yes (ver 2.2.7) + Use CMU 1394: no + Use V4L2: no + Use directshow: no + Use OpenCV: yes (ver 4.8.1) + Use FLIR Flycapture: no + Use Basler Pylon: no + Use IDS uEye: no + + RGB-D sensors: + Use Realsense: no + Use Realsense2: yes (ver 2.54.1) + Use Occipital Structure: no + Use Kinect: no + \- Use libfreenect: no + \- Use libusb-1: yes (ver 1.0.26) + \- Use pthread: yes + Use PCL: no + \- Use VTK: no + + F/T sensors: + Use atidaq (built-in): no + Use comedi: no + Use IIT SDK: no + + Mocap: + Use Qualisys: no + Use Vicon: no + + Detection: + Use zbar: no + Use dmtx: no + Use AprilTag (built-in): yes (ver 3.1.1) + \- Use AprilTag big family: no + + Misc: + Use Clipper (built-in): yes (ver 6.4.2) + Use pugixml (built-in): yes (ver 1.9.0) + Use libxml2: yes (ver 2.11.5) + Use json (nlohmann): no + + Optimization: + Use OpenMP: yes + Use pthread: yes + Use pthread (built-in): no + Use Simd (built-in): yes (ver 4.9.109) + + DNN: + Use CUDA Toolkit: no + Use TensorRT: no + + Documentation: + Use doxygen: no + \- Use mathjax: no + + Tests and samples: + Use catch2 (built-in): yes (ver 2.13.7) + Tests: yes + Demos: yes + Examples: yes + Tutorials: yes + Dataset found: no + + Library dirs: + Eigen3 include dir: $PREFIX/share/eigen3/cmake + MKL include dir: $PREFIX/include + OpenCV dir: $PREFIX/lib/cmake/opencv4 + + Install path: $PREFIX + +========================================================== +\endverbatim + +\subsection conda_package_config_osx_64 Configuration for the OSX-64 Conda package + +\verbatim +========================================================== +General configuration information for ViSP 3.6.0 + + Version control: unknown + + Platform: + Timestamp: 2023-10-23T18:01:45Z + Host: Darwin 20.6.0 x86_64 + CMake: 3.27.6 + CMake generator: Unix Makefiles + CMake build tool: $BUILD_PREFIX/bin/make + Configuration: Release + + System information: + Number of CPU logical cores: 3 + Number of CPU physical cores: 3 + Total physical memory (in MiB): 14336 + OS name: macOS + OS release: 11.7.10 + OS version: 20G1427 + OS platform: x86_64 + CPU name: Intel(R) Xeon(R) CPU E5-1650 v2 @ 3.50GHz + Is the CPU 64-bit? yes + Does the CPU have FPU? yes + CPU optimization: SSE2 SSE3 SSSE3 + + C/C++: + Built as dynamic libs?: yes + C++ Compiler: $BUILD_PREFIX/bin/x86_64-apple-darwin13.4.0-clang++ (ver 16.0.6) + C++ flags (Release): -march=core2 -mtune=haswell -mssse3 -ftree-vectorize -fPIC -fPIE -fstack-protector-strong -O2 -pipe -stdlib=libc++ -fvisibility-inlines-hidden -fmessage-length=0 -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -D_LIBCPP_DISABLE_AVAILABILITY -Wall -Wextra -fopenmp=libomp -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG + C++ flags (Debug): -march=core2 -mtune=haswell -mssse3 -ftree-vectorize -fPIC -fPIE -fstack-protector-strong -O2 -pipe -stdlib=libc++ -fvisibility-inlines-hidden -fmessage-length=0 -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -D_LIBCPP_DISABLE_AVAILABILITY -Wall -Wextra -fopenmp=libomp -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g + C Compiler: $BUILD_PREFIX/bin/x86_64-apple-darwin13.4.0-clang + C flags (Release): -march=core2 -mtune=haswell -mssse3 -ftree-vectorize -fPIC -fPIE -fstack-protector-strong -O2 -pipe -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -Wall -Wextra -fopenmp=libomp -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG + C flags (Debug): -march=core2 -mtune=haswell -mssse3 -ftree-vectorize -fPIC -fPIE -fstack-protector-strong -O2 -pipe -isystem $PREFIX/include -fdebug-prefix-map=$SRC_DIR=/usr/local/src/conda/visp-3.6.0 -fdebug-prefix-map=$PREFIX=/usr/local/src/conda-prefix -Wall -Wextra -fopenmp=libomp -std=c++17 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g + Linker flags (Release): -Wl,-pie -Wl,-headerpad_max_install_names -Wl,-dead_strip_dylibs -Wl,-rpath,$PREFIX/lib -L$PREFIX/lib + Linker flags (Debug): -Wl,-pie -Wl,-headerpad_max_install_names -Wl,-dead_strip_dylibs -Wl,-rpath,$PREFIX/lib -L$PREFIX/lib + Use cxx standard: 17 + + ViSP modules: + To be built: core gui imgproc io java_bindings_generator klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi + Disabled: - + Disabled by dependency: - + Unavailable: java + + Python (for build): $PREFIX/bin/python + + Java: + ant: no + JNI: /Users/runner/hostedtoolcache/Java_Temurin-Hotspot_jdk/17.0.8-101/x64/Contents/Home/include /Users/runner/hostedtoolcache/Java_Temurin-Hotspot_jdk/17.0.8-101/x64/Contents/Home/include/darwin /Users/runner/hostedtoolcache/Java_Temurin-Hotspot_jdk/17.0.8-101/x64/Contents/Home/include + + Build options: + Build deprecated: yes + Build with moment combine: no + + OpenCV: + Version: 4.8.1 + Modules: calib3d core dnn features2d flann gapi highgui imgcodecs imgproc ml objdetect photo stitching video videoio alphamat aruco bgsegm bioinspired ccalib datasets dnn_objdetect dnn_superres dpm face freetype fuzzy hdf hfs img_hash intensity_transform line_descriptor mcc optflow phase_unwrapping plot quality rapid reg rgbd saliency shape stereo structured_light superres surface_matching text tracking videostab wechat_qrcode xfeatures2d ximgproc xobjdetect xphoto + OpenCV dir: $PREFIX/lib/cmake/opencv4 + + Mathematics: + Blas/Lapack: yes + \- Use MKL: no + \- Use OpenBLAS: no + \- Use Atlas: no + \- Use Netlib: yes (ver n/a) + \- Use GSL: no + \- Use Lapack (built-in): no + Use Eigen3: yes (ver 3.4.0) + Use OpenCV: yes (ver 4.8.1) + + Simulator: + Ogre simulator: + \- Use Ogre3D: no + \- Use OIS: no + Coin simulator: + \- Use Coin3D: no + \- Use SoWin: no + \- Use SoXt: no + \- Use SoQt: no + \- Use Qt5: no + \- Use Qt4: no + \- Use Qt3: no + + Media I/O: + Use JPEG: yes (ver 80) + Use PNG: yes (ver 1.6.39) + \- Use ZLIB: yes (ver 1.2.13) + Use OpenCV: yes (ver 4.8.1) + Use stb_image (built-in): yes (ver 2.27.0) + Use TinyEXR (built-in): yes (ver 1.0.2) + + Real robots: + Use Afma4: no + Use Afma6: no + Use Franka: no + Use Viper650: no + Use Viper850: no + Use ur_rtde: no + Use Kinova Jaco: no + Use aria (Pioneer): no + Use PTU46: no + Use Biclops PTU: no + Use Flir PTU SDK: no + Use MAVSDK: no + Use Parrot ARSDK: no + \-Use ffmpeg: no + Use Virtuose: no + Use qbdevice (built-in): yes (ver 2.6.0) + Use takktile2 (built-in): yes (ver 1.0.0) + + GUI: + Use X11: yes + Use GTK: no + Use OpenCV: yes (ver 4.8.1) + Use GDI: no + Use Direct3D: no + + Cameras: + Use DC1394-2.x: yes (ver 2.2.7) + Use CMU 1394: no + Use V4L2: no + Use directshow: no + Use OpenCV: yes (ver 4.8.1) + Use FLIR Flycapture: no + Use Basler Pylon: no + Use IDS uEye: no + + RGB-D sensors: + Use Realsense: no + Use Realsense2: yes (ver 2.54.1) + Use Occipital Structure: no + Use Kinect: no + \- Use libfreenect: no + \- Use libusb-1: no + \- Use pthread: yes + Use PCL: no + \- Use VTK: no + + F/T sensors: + Use atidaq (built-in): no + Use comedi: no + Use IIT SDK: no + + Mocap: + Use Qualisys: no + Use Vicon: no + + Detection: + Use zbar: no + Use dmtx: no + Use AprilTag (built-in): yes (ver 3.1.1) + \- Use AprilTag big family: no + + Misc: + Use Clipper (built-in): yes (ver 6.4.2) + Use pugixml (built-in): yes (ver 1.9.0) + Use libxml2: no + Use json (nlohmann): no + + Optimization: + Use OpenMP: yes + Use pthread: yes + Use pthread (built-in): no + Use Simd (built-in): yes (ver 4.9.109) + + DNN: + Use CUDA Toolkit: no + Use TensorRT: no + + Documentation: + Use doxygen: no + \- Use mathjax: no + + Tests and samples: + Use catch2 (built-in): yes (ver 2.13.7) + Tests: yes + Demos: yes + Examples: yes + Tutorials: yes + Dataset found: no + + Library dirs: + Eigen3 include dir: $PREFIX/share/eigen3/cmake + OpenCV dir: $PREFIX/lib/cmake/opencv4 + + Install path: $PREFIX + +========================================================== +\endverbatim + +\subsection conda_package_config_windows_64 Configuration for the Windows-64 Conda package + +\verbatim +========================================================== +General configuration information for ViSP 3.6.0 + + Version control: unknown + + Platform: + Timestamp: 2023-10-23T18:06:34Z + Host: Windows 10.0.20348 AMD64 + CMake: 3.27.6 + CMake generator: Ninja + CMake build tool: D:/bld/visp_1698083699413/_build_env/Library/bin/ninja.exe + MSVC: 1929 + Configuration: Release + + System information: + Number of CPU logical cores: 2 + Number of CPU physical cores: 2 + Total physical memory (in MiB): 7167 + OS name: Windows + OS release: DataCenter Server + OS version: (Build 20348) + OS platform: AMD64 + CPU name: Unknown P6 family + Is the CPU 64-bit? yes + Does the CPU have FPU? yes + CPU optimization: + + C/C++: + Built as dynamic libs?: yes + C++ Compiler: D:/bld/visp_1698083699413/_build_env/Library/bin/clang-cl.exe (ver 17.0.3) + C++ flags (Release): /DWIN32 /D_WINDOWS /W3 /GR /EHsc /MP2 -Wall -Wextra -Xclang -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 /Gy /bigobj /MD /O2 /Ob2 /DNDEBUG + C++ flags (Debug): /DWIN32 /D_WINDOWS /W3 /GR /EHsc /MP2 -Wall -Wextra -Xclang -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 /Gy /bigobj /MDd /Zi /Ob0 /Od /RTC1 + C Compiler: D:/bld/visp_1698083699413/_build_env/Library/bin/clang-cl.exe + C flags (Release): /DWIN32 /D_WINDOWS /W3 /MP2 -Wall -Wextra -Xclang -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 /MD /O2 /Ob2 /DNDEBUG + C flags (Debug): /DWIN32 /D_WINDOWS /W3 /MP2 -Wall -Wextra -Xclang -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 /MDd /Zi /Ob0 /Od /RTC1 + Linker flags (Release): /machine:x64 /INCREMENTAL:NO + Linker flags (Debug): /machine:x64 /debug /INCREMENTAL /ignore:4099 + Use cxx standard: 17 + + ViSP modules: + To be built: core gui imgproc io java_bindings_generator klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi + Disabled: - + Disabled by dependency: - + Unavailable: java + + Windows RT support: no + + Python (for build): D:/bld/visp_1698083699413/_h_env/python.exe + + Java: + ant: no + JNI: C:/Program Files/Microsoft/jdk-11.0.16.101-hotspot/include C:/Program Files/Microsoft/jdk-11.0.16.101-hotspot/include/win32 C:/Program Files/Microsoft/jdk-11.0.16.101-hotspot/include + + Build options: + Build deprecated: yes + Build with moment combine: no + + OpenCV: + Version: 4.8.1 + Modules: calib3d core dnn features2d flann gapi highgui imgcodecs imgproc ml objdetect photo stitching video videoio alphamat aruco bgsegm ccalib cvv datasets dnn_objdetect dnn_superres dpm face freetype fuzzy hdf hfs img_hash intensity_transform line_descriptor mcc optflow phase_unwrapping plot quality rapid reg rgbd saliency shape stereo structured_light superres surface_matching text tracking videostab wechat_qrcode xfeatures2d ximgproc xobjdetect xphoto + OpenCV dir: D:/bld/visp_1698083699413/_h_env/Library/cmake + + Mathematics: + Blas/Lapack: yes + \- Use MKL: no + \- Use OpenBLAS: no + \- Use Atlas: no + \- Use Netlib: no + \- Use GSL: no + \- Use Lapack (built-in): yes (ver 3.2.1) + Use Eigen3: yes (ver 3.4.0) + Use OpenCV: yes (ver 4.8.1) + + Simulator: + Ogre simulator: + \- Use Ogre3D: no + \- Use OIS: no + Coin simulator: + \- Use Coin3D: no + \- Use SoWin: no + \- Use SoXt: no + \- Use SoQt: no + \- Use Qt5: no + \- Use Qt4: no + \- Use Qt3: no + + Media I/O: + Use JPEG: yes (ver 80) + Use PNG: yes (ver 1.6.39) + \- Use ZLIB: yes (ver 1.2.13) + Use OpenCV: yes (ver 4.8.1) + Use stb_image (built-in): yes (ver 2.27.0) + Use TinyEXR (built-in): yes (ver 1.0.2) + + Real robots: + Use Afma4: no + Use Afma6: no + Use Franka: no + Use Viper650: no + Use Viper850: no + Use ur_rtde: no + Use Kinova Jaco: no + Use aria (Pioneer): no + Use PTU46: no + Use Biclops PTU: no + Use Flir PTU SDK: no + Use MAVSDK: no + Use Parrot ARSDK: no + \-Use ffmpeg: no + Use Virtuose: no + Use qbdevice (built-in): yes (ver 2.6.0) + Use takktile2 (built-in): no + + GUI: + Use X11: no + Use GTK: no + Use OpenCV: yes (ver 4.8.1) + Use GDI: yes + Use Direct3D: no + + Cameras: + Use DC1394-2.x: no + Use CMU 1394: no + Use V4L2: no + Use directshow: no + Use OpenCV: yes (ver 4.8.1) + Use FLIR Flycapture: no + Use Basler Pylon: no + Use IDS uEye: no + + RGB-D sensors: + Use Realsense: no + Use Realsense2: no + Use Occipital Structure: no + Use Kinect: no + \- Use libfreenect: no + \- Use libusb-1: no + \- Use pthread: no + Use PCL: no + \- Use VTK: no + + F/T sensors: + Use atidaq (built-in): no + Use comedi: no + Use IIT SDK: no + + Mocap: + Use Qualisys: no + Use Vicon: no + + Detection: + Use zbar: no + Use dmtx: no + Use AprilTag (built-in): yes (ver 3.1.1) + \- Use AprilTag big family: no + + Misc: + Use Clipper (built-in): yes (ver 6.4.2) + Use pugixml (built-in): yes (ver 1.9.0) + Use libxml2: no + Use json (nlohmann): no + + Optimization: + Use OpenMP: yes + Use pthread: no + Use pthread (built-in): yes (ver 3.0.1) + Use Simd (built-in): yes (ver 4.9.109) + + DNN: + Use CUDA Toolkit: no + Use TensorRT: no + + Documentation: + Use doxygen: no + \- Use mathjax: no + + Tests and samples: + Use catch2 (built-in): yes (ver 2.13.7) + Tests: yes + Demos: yes + Examples: yes + Tutorials: yes + Dataset found: no + + Library dirs: + Eigen3 include dir: D:/bld/visp_1698083699413/_h_env/Library/share/eigen3/cmake + OpenCV dir: D:/bld/visp_1698083699413/_h_env/Library/cmake + + Install path: D:/bld/visp_1698083699413/_h_env/Library + +========================================================== +\endverbatim + +\section install_conda_package_next Next tutorial + +You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project. + +*/ diff --git a/doc/tutorial/tutorial-install.dox b/doc/tutorial/tutorial-install.dox index 6c5a1edd5a..a57e62f79a 100644 --- a/doc/tutorial/tutorial-install.dox +++ b/doc/tutorial/tutorial-install.dox @@ -17,6 +17,7 @@ This page introduces the user to the way to install ViSP from existing prebuild - \subpage tutorial-install-ubuntu-package
In this first tutorial you will learn how to install ViSP prebuilt library SDK from Ubuntu or Debian packages. - \subpage tutorial-install-archlinux-package
In this tutorial you will learn how to install ViSP prebuilt SDK for Arch Linux. - \subpage tutorial-install-osx-homebrew-package
In this tutorial you will learn how to install ViSP prebuilt SDK on OSX with Homebrew. +- \subpage tutorial-install-conda-package
In this tutorial you will learn how to install ViSP prebuilt SDK using Conda (compatible with Linux, OSX and Windows). */ diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 3bb9d94e41..592d13f381 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -91,6 +91,7 @@ visp_add_subdirectory(servo-franka REQUIRED_DEPS visp_core visp_robo visp_add_subdirectory(servo-kinova REQUIRED_DEPS visp_core visp_robot visp_vision visp_io visp_gui visp_vs visp_visual_features visp_detection) visp_add_subdirectory(servo-pioneer REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui) visp_add_subdirectory(servo-pixhawk REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui visp_detection visp_sensor) +visp_add_subdirectory(servo-pololu-ptu REQUIRED_DEPS visp_core visp_robot visp_vision visp_gui visp_vs visp_visual_features visp_detection visp_sensor) visp_add_subdirectory(servo-ptu46 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui) visp_add_subdirectory(servo-universal-robots REQUIRED_DEPS visp_core visp_robot visp_io) visp_add_subdirectory(servo-viper650 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui) diff --git a/example/servo-afma6/servoAfma6MegaposePBVS.cpp b/example/servo-afma6/servoAfma6MegaposePBVS.cpp index 25530a7f6d..1fde8eba31 100644 --- a/example/servo-afma6/servoAfma6MegaposePBVS.cpp +++ b/example/servo-afma6/servoAfma6MegaposePBVS.cpp @@ -77,8 +77,8 @@ #include #include - -#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ +// Check if std:c++17 or higher +#if defined(VISP_HAVE_REALSENSE2) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_MODULE_DNN_TRACKER) #include @@ -455,8 +455,8 @@ int main() #if !defined(VISP_HAVE_REALSENSE2) std::cout << "Install librealsense-2.x" << std::endl; #endif -#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_17) - std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=17)." << std::endl; +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) + std::cout << "Build ViSP with c++17 or higher compiler flag (cmake -DUSE_CXX_STANDARD=17)." << std::endl; #endif #if !defined(VISP_HAVE_AFMA6) std::cout << "ViSP is not built with Afma-6 robot support..." << std::endl; diff --git a/example/servo-pixhawk/sendMocapToPixhawk.cpp b/example/servo-pixhawk/sendMocapToPixhawk.cpp index c2b9d296bf..67cf9fdc32 100644 --- a/example/servo-pixhawk/sendMocapToPixhawk.cpp +++ b/example/servo-pixhawk/sendMocapToPixhawk.cpp @@ -44,7 +44,8 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \ (defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON)) #include @@ -92,7 +93,8 @@ bool mocap_sdk_loop(std::mutex &lock, bool qualisys, bool opt_verbose, bool opt_ std::cout << "ERROR : Qualisys not found."; return false; #endif - } else { + } + else { #ifdef VISP_HAVE_VICON mocap = std::make_shared(); #else @@ -120,11 +122,13 @@ bool mocap_sdk_loop(std::mutex &lock, bool qualisys, bool opt_verbose, bool opt_ if (!mocap->getBodiesPose(body_poses_enu_M_flu, opt_all_bodies)) { g_quit = true; } - } else { + } + else { vpHomogeneousMatrix enu_M_flu; if (!mocap->getSpecificBodyPose(opt_onlyBody, enu_M_flu)) { g_quit = true; - } else { + } + else { body_poses_enu_M_flu[opt_onlyBody] = enu_M_flu; } } @@ -132,7 +136,7 @@ bool mocap_sdk_loop(std::mutex &lock, bool qualisys, bool opt_verbose, bool opt_ lock.lock(); internal_mavlink_failure = mavlink_failure; current_body_poses_enu_M_flu = - body_poses_enu_M_flu; // Now we send directly the poses in the ENU global reference frame. + body_poses_enu_M_flu; // Now we send directly the poses in the ENU global reference frame. lock.unlock(); } return true; @@ -148,7 +152,7 @@ int top(const std::string &connection_info, std::map] [-ob]" - << " [--mocap-system /] [-ms /]" - << " [--device ] [-d]" - << " [--server-address ] [-sa]" - << " [--verbose] [-v]" - << " [--help] [-h]" << std::endl - << std::endl; + << " " << argv[0] << " [--only-body ] [-ob]" + << " [--mocap-system /] [-ms /]" + << " [--device ] [-d]" + << " [--server-address ] [-sa]" + << " [--verbose] [-v]" + << " [--help] [-h]" << std::endl + << std::endl; std::cout << "DESCRIPTION" << std::endl - << "MANDATORY PARAMETERS :" << std::endl - << " --only-body " << std::endl - << " Name of the specific body you want to be displayed." << std::endl - << std::endl - << "OPTIONAL PARAMETERS (DEFAULT VALUES)" << std::endl - << " --mocap-system, -ms" << std::endl - << " Specify the name of the mocap system : 'qualisys' / 'q' or 'vicon'/ 'v'." << std::endl - << " Default: Qualisys mode." << std::endl - << std::endl - << " --device , -d" << std::endl - << " String giving us all the informations necessary for connection." << std::endl - << " Default: serial:///dev/ttyUSB0 ." << std::endl - << " UDP example: udp://192.168.30.111:14540 (udp://IP:Port) ." << std::endl - << std::endl - << " --server-address
, -sa" << std::endl - << " Mocap server address." << std::endl - << " Default for Qualisys: 192.168.34.42 ." << std::endl - << " Default for Vicon: 192.168.34.1 ." << std::endl - << std::endl - << " --verbose, -v" << std::endl - << " Enable verbose mode." << std::endl - << std::endl - << " --help, -h" << std::endl - << " Print this helper message." << std::endl - << std::endl; + << "MANDATORY PARAMETERS :" << std::endl + << " --only-body " << std::endl + << " Name of the specific body you want to be displayed." << std::endl + << std::endl + << "OPTIONAL PARAMETERS (DEFAULT VALUES)" << std::endl + << " --mocap-system, -ms" << std::endl + << " Specify the name of the mocap system : 'qualisys' / 'q' or 'vicon'/ 'v'." << std::endl + << " Default: Qualisys mode." << std::endl + << std::endl + << " --device , -d" << std::endl + << " String giving us all the informations necessary for connection." << std::endl + << " Default: serial:///dev/ttyUSB0 ." << std::endl + << " UDP example: udp://192.168.30.111:14540 (udp://IP:Port) ." << std::endl + << std::endl + << " --server-address
, -sa" << std::endl + << " Mocap server address." << std::endl + << " Default for Qualisys: 192.168.34.42 ." << std::endl + << " Default for Vicon: 192.168.34.1 ." << std::endl + << std::endl + << " --verbose, -v" << std::endl + << " Enable verbose mode." << std::endl + << std::endl + << " --help, -h" << std::endl + << " Print this helper message." << std::endl + << std::endl; if (error) { std::cout << "Error" << std::endl - << " " - << "Unsupported parameter " << argv[error] << std::endl; + << " " + << "Unsupported parameter " << argv[error] << std::endl; } } @@ -230,31 +234,40 @@ void parse_commandline(int argc, char **argv, bool &qualisys, std::string &conne if (std::string(argv[i]) == "--only-body" || std::string(argv[i]) == "-ob") { only_body = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--mocap-system" || std::string(argv[i]) == "-ms") { + } + else if (std::string(argv[i]) == "--mocap-system" || std::string(argv[i]) == "-ms") { std::string mode = std::string(argv[i + 1]); if (mode == "qualisys" || mode == "q") { qualisys = true; - } else if (mode == "vicon" || mode == "v") { + } + else if (mode == "vicon" || mode == "v") { qualisys = false; - } else { + } + else { std::cout << "ERROR : System not recognized, exiting." << std::endl; throw EXIT_FAILURE; } i++; - } else if (std::string(argv[i]) == "--device" || std::string(argv[i]) == "-d") { + } + else if (std::string(argv[i]) == "--device" || std::string(argv[i]) == "-d") { connection_info = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") { + } + else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") { server_address = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--all-bodies") { + } + else if (std::string(argv[i]) == "--all-bodies") { all_bodies = true; - } else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { + } + else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { verbose = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { usage(argv, 0); throw EXIT_SUCCESS; - } else { + } + else { usage(argv, i); throw EXIT_FAILURE; } @@ -289,7 +302,8 @@ int main(int argc, char **argv) if (opt_qualisys && opt_serverAddress == "") { opt_serverAddress = "192.168.30.42"; - } else if (!opt_qualisys && opt_serverAddress == "") { + } + else if (!opt_qualisys && opt_serverAddress == "") { opt_serverAddress = "192.168.30.1"; } @@ -304,8 +318,8 @@ int main(int argc, char **argv) bool mavlink_failure = false; std::thread mocap_thread([&lock, &opt_qualisys, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody, ¤t_body_poses_enu_M_flu, &mocap_failure, &mavlink_failure]() { - mocap_sdk_loop(lock, opt_qualisys, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody, - current_body_poses_enu_M_flu, mocap_failure, mavlink_failure); + mocap_sdk_loop(lock, opt_qualisys, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody, + current_body_poses_enu_M_flu, mocap_failure, mavlink_failure); }); if (mocap_failure) { std::cout << "Mocap connexion failure. Check mocap server IP address" << std::endl; @@ -318,7 +332,8 @@ int main(int argc, char **argv) try { int result = top(opt_connectionInfo, current_body_poses_enu_M_flu, lock, mocap_failure); return result; - } catch (int error) { + } + catch (int error) { fprintf(stderr, "mavlink_control threw exception %i \n", error); lock.lock(); mavlink_failure = true; @@ -331,7 +346,8 @@ int main(int argc, char **argv) mavlink_thread.join(); if (mocap_failure) { return EXIT_FAILURE; - } else { + } + else { return EXIT_SUCCESS; } } @@ -342,18 +358,18 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif #if !(defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON)) std::cout << "\nThis example requires data from a Qualisys or Vicon mocap system. You should install it, configure " - "and rebuid ViSP.\n" - << std::endl; + "and rebuid ViSP.\n" + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp b/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp index f7d14d15ae..54e9960052 100644 --- a/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp +++ b/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp @@ -37,7 +37,8 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && defined(VISP_HAVE_REALSENSE2) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && defined(VISP_HAVE_REALSENSE2) #include #include @@ -110,7 +111,8 @@ int main(int argc, char **argv) if (std::string(argv[i]) == "--co" && i + 1 < argc) { opt_connecting_info = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--distance-to-tag" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--distance-to-tag" && i + 1 < argc) { opt_distance_to_tag = std::atof(argv[i + 1]); if (opt_distance_to_tag <= 0) { std::cout << "Error : invalid distance to tag." << std::endl << "See " << argv[0] << " --help" << std::endl; @@ -119,59 +121,64 @@ int main(int argc, char **argv) opt_has_distance_to_tag = true; i++; - } else if (std::string(argv[i]) == "--display-fps" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--display-fps" && i + 1 < argc) { opt_display_fps = std::stoi(std::string(argv[i + 1])); i++; - } else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { + } + else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { opt_verbose = true; - } else { + } + else { std::cout << "Error : unknown parameter " << argv[i] << std::endl - << "See " << argv[0] << " --help" << std::endl; + << "See " << argv[0] << " --help" << std::endl; return EXIT_FAILURE; } } - } else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) { + } + else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) { std::cout << "\nUsage:\n" - << " " << argv[0] - << " [--tag-size ] [--co ] [--distance-to-tag ]" - << " [--display-fps ] [--verbose] [-v] [--help] [-h]\n" - << std::endl - << "Description:\n" - << " --tag-size \n" - << " The size of the tag to detect in meters, required.\n\n" - << " --co \n" - << " - UDP: udp://[host][:port]\n" - << " - TCP: tcp://[host][:port]\n" - << " - serial: serial://[path][:baudrate]\n" - << " - Default: udp://192.168.30.111:14552).\n\n" - << " --distance-to-tag \n" - << " The desired distance to the tag in meters (default: 1 meter).\n\n" - << " --display-fps \n" - << " The desired fps rate for the video display (default: 10 fps).\n\n" - << " --verbose, -v\n" - << " Enables verbosity (drone information messages and velocity commands\n" - << " are then displayed).\n\n" - << " --help, -h\n" - << " Print help message.\n" - << std::endl; + << " " << argv[0] + << " [--tag-size ] [--co ] [--distance-to-tag ]" + << " [--display-fps ] [--verbose] [-v] [--help] [-h]\n" + << std::endl + << "Description:\n" + << " --tag-size \n" + << " The size of the tag to detect in meters, required.\n\n" + << " --co \n" + << " - UDP: udp://[host][:port]\n" + << " - TCP: tcp://[host][:port]\n" + << " - serial: serial://[path][:baudrate]\n" + << " - Default: udp://192.168.30.111:14552).\n\n" + << " --distance-to-tag \n" + << " The desired distance to the tag in meters (default: 1 meter).\n\n" + << " --display-fps \n" + << " The desired fps rate for the video display (default: 10 fps).\n\n" + << " --verbose, -v\n" + << " Enables verbosity (drone information messages and velocity commands\n" + << " are then displayed).\n\n" + << " --help, -h\n" + << " Print help message.\n" + << std::endl; return EXIT_SUCCESS; - } else { + } + else { std::cout << "Error : tag size parameter required." << std::endl << "See " << argv[0] << " --help" << std::endl; return EXIT_FAILURE; } std::cout << std::endl - << "WARNING:" << std::endl - << " - This program does no sensing or avoiding of obstacles, " << std::endl - << " the drone WILL collide with any objects in the way! Make sure the " << std::endl - << " drone has approximately 3 meters of free space on all sides." << std::endl - << " - The drone uses a forward-facing camera for Apriltag detection," << std::endl - << " make sure the drone flies above a non-uniform flooring," << std::endl - << " or its movement will be inacurate and dangerous !" << std::endl - << std::endl; - - // Connect to the drone + << "WARNING:" << std::endl + << " - This program does no sensing or avoiding of obstacles, " << std::endl + << " the drone WILL collide with any objects in the way! Make sure the " << std::endl + << " drone has approximately 3 meters of free space on all sides." << std::endl + << " - The drone uses a forward-facing camera for Apriltag detection," << std::endl + << " make sure the drone flies above a non-uniform flooring," << std::endl + << " or its movement will be inacurate and dangerous !" << std::endl + << std::endl; + +// Connect to the drone vpRobotMavsdk drone(opt_connecting_info); if (drone.isRunning()) { @@ -260,7 +267,7 @@ int main(int argc, char **argv) vpRotationMatrix c1Rc(c1_rxyz_c); // Rotation between (c1) and (c) vpHomogeneousMatrix c1Mc(vpTranslationVector(), c1Rc); // Homogeneous matrix between (c1) and (c) - vpRotationMatrix c1Re{1, 0, 0, 0, 0, 1, 0, -1, 0}; // Rotation between (c1) and (e) + vpRotationMatrix c1Re { 1, 0, 0, 0, 0, 1, 0, -1, 0 }; // Rotation between (c1) and (e) vpTranslationVector c1te(0, -0.03, -0.07); // Translation between (c1) and (e) vpHomogeneousMatrix c1Me(c1te, c1Re); // Homogeneous matrix between (c1) and (e) @@ -281,8 +288,8 @@ int main(int argc, char **argv) double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.); // Define the desired polygon corresponding the the AprilTag CLOCKWISE - double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.}; - double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.}; + double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. }; + double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. }; std::vector vec_P, vec_P_d; for (int i = 0; i < 4; i++) { @@ -491,14 +498,16 @@ int main(int argc, char **argv) false); } - } else { + } + else { std::stringstream sserr; sserr << "Failed to detect an Apriltag, or detected multiple ones"; if (condition) { vpDisplay::displayText(I, 120, 20, sserr.str(), vpColor::red); vpDisplay::flush(I); - } else { + } + else { std::cout << sserr.str() << std::endl; } #ifdef CONTROL_UAV @@ -510,7 +519,7 @@ int main(int argc, char **argv) { std::stringstream ss; ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") - << ", right click to quit."; + << ", right click to quit."; vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red); } vpDisplay::flush(I); @@ -548,11 +557,13 @@ int main(int argc, char **argv) } return EXIT_SUCCESS; - } else { + } + else { std::cout << "ERROR : failed to setup drone control." << std::endl; return EXIT_FAILURE; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Caught an exception: " << e << std::endl; return EXIT_FAILURE; } @@ -564,17 +575,17 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif #ifndef VISP_HAVE_REALSENSE2 std::cout << "\nThis example requires librealsense2 library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/example/servo-pololu-ptu/CMakeLists.txt b/example/servo-pololu-ptu/CMakeLists.txt new file mode 100644 index 0000000000..10c599877d --- /dev/null +++ b/example/servo-pololu-ptu/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################# +# +# 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: +# ViSP configuration file. +# +############################################################################# + +cmake_minimum_required(VERSION 3.5) + +project(example-servo-biclops) + +find_package(VISP REQUIRED visp_core visp_detection visp_vs visp_robot visp_sensor visp_gui) + +set(example_cpp + servoPololuPtuPoint2DJointVelocity.cpp +) + +foreach(cpp ${example_cpp}) + visp_add_target(${cpp}) + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "examples") + endif() +endforeach() diff --git a/example/servo-pololu-ptu/servoPololuPtuPoint2DJointVelocity.cpp b/example/servo-pololu-ptu/servoPololuPtuPoint2DJointVelocity.cpp new file mode 100644 index 0000000000..b38d3ca367 --- /dev/null +++ b/example/servo-pololu-ptu/servoPololuPtuPoint2DJointVelocity.cpp @@ -0,0 +1,345 @@ +/* + * 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: + * tests the control law + * eye-in-hand control + * velocity computed in joint + */ + +/*! + * \file servoPololuPtuPoint2DJointVelocity.cpp + * \example servoPololuPtuPoint2DJointVelocity.cpp + * + * Example of eye-in-hand control law. We control here a real robot, a pan-tilt head controlled using a Pololu Maestro + * board where pan axis servo a connected to channel 0 and tilt axis to channel 1. The velocity is computed + * in joint. The visual feature is a 2D point corresponding to the center of gravity of an AprilTag. + * A Realsense camera is mounted on the pan-tilt unit. + */ + +#include + +#include + +#if defined(VISP_HAVE_POLOLU) && defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void usage(const char **argv, int error, const std::string &device, int baudrate) +{ + std::cout << "Name" << std::endl + << " Example of eye-in-hand control law. We control here a real robot, a pan-tilt unit" << std::endl + << " controlled using a Pololu Maestro board equipped.The PTU is equipped with a Realsense" << std::endl + << " camera mounted on its end-effector.The velocity to apply to the PT head is a joint" << std::endl + << " velocity.The visual feature is a point corresponding to the center of gravity" << std::endl + << " of an AprilTag." << std::endl + << std::endl; + std::cout << "Synopsis" << std::endl + << " " << argv[0] << " [--device ] [--baud ] [--verbose, -v] [--help, -h]" << std::endl + << std::endl; + std::cout << "Description" << std::endl + << " --device Device name." << std::endl + << " Default: " << device << std::endl + << std::endl + << " --baud Serial link baud rate." << std::endl + << " Default: " << baudrate << std::endl + << std::endl + << " --verbose, -v Enable verbosity." << std::endl + << std::endl + << " --help, -h Print this helper message." << std::endl + << std::endl; + if (error) { + std::cout << "Error" << std::endl + << " " + << "Unsupported parameter " << argv[error] << std::endl; + } +} + +int main(int argc, const char **argv) +{ +#ifdef _WIN32 + std::string opt_device = "COM4"; +#else + std::string opt_device = "/dev/ttyACM0"; + // Example for Mac OS, the Maestro creates two devices, use the one with the lowest number (the command port) + //std::string opt_device = "/dev/cu.usbmodem00031501"; +#endif + int opt_baudrate = 38400; + bool opt_verbose = false; + + for (int i = 1; i < argc; i++) { + if (std::string(argv[i]) == "--device" && i + 1 < argc) { + opt_device = std::string(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { + opt_verbose = true; + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + usage(argv, 0, opt_device, opt_baudrate); + return EXIT_SUCCESS; + } + else { + usage(argv, i, opt_device, opt_baudrate); + return EXIT_FAILURE; + } + } + + try { + // Creating the servo object on channel 0 + vpRobotPololuPtu robot(opt_device, opt_baudrate, opt_verbose); + + /* + * Pololu PTU has the following axis orientation (rear view) + * + * tilt + <---- (end-effector-frame) + * | + * \/ pan + + * + * The PTU end-effector-frame is the following (rear view) + * + * /\ x + * | + * (e) ----> y + * + * The camera frame attached to the PT unit is the following (rear view) + * + * (c) ----> x + * | + * \/ y + * + * The corresponding cRe (camera to end-effector rotation matrix) is then the following + * + * ( 0 1 0) + * cRe = (-1 0 0) + * ( 0 0 1) + * + * Translation cte (camera to end-effector) can be neglected + * + * (0) + * cte = (0) + * (0) + */ + + vpRotationMatrix cRe({ 0, 1, 0, -1, 0, 0, 0, 0, 1 }); + vpTranslationVector cte; // By default set to 0 + + // Robot Jacobian (expressed in the end-effector frame) + vpMatrix eJe; + // Camera to end-effector frame transformation + vpHomogeneousMatrix cMe(cte, cRe); + // Velocity twist transformation to express a velocity from end-effector to camera frame + vpVelocityTwistMatrix cVe(cMe); + + vpColVector q(robot.getNDof()); + q = 0; + std::cout << "Move PT to initial position: " << q.t() << std::endl; + robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); + robot.setPositioningVelocityPercentage(10.f); + robot.setPosition(vpRobot::JOINT_STATE, q); + + vpTime::wait(1500); // TODO make setPosition() blocking + + std::cout << "Min velocity resolution: " << vpMath::deg(robot.getAngularVelocityResolution()) << " deg/s" << std::endl; + + // Initialize grabber + vpRealSense2 g; + rs2::config config; + config.disable_stream(RS2_STREAM_DEPTH); + config.disable_stream(RS2_STREAM_INFRARED); + config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30); + g.open(config); + + std::cout << "Read camera parameters from Realsense device" << std::endl; + vpCameraParameters cam; + cam = g.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion); + + vpImage I; + g.acquire(I); + + // We open a window using either X11 or GTK or GDI. + // Its size is automatically defined by the image (I) size +#if defined(VISP_HAVE_X11) + vpDisplayX display(I, 100, 100, "Display X..."); +#elif defined(VISP_HAVE_GTK) + vpDisplayGTK display(I, 100, 100, "Display GTK..."); +#elif defined(VISP_HAVE_GDI) + vpDisplayGDI display(I, 100, 100, "Display GDI..."); +#endif + + vpDisplay::display(I); + vpDisplay::flush(I); + + vpDetectorAprilTag detector; + + vpServo task; + + // Create current and desired point visual feature + vpFeaturePoint p, pd; + // Sets the desired position of the visual feature + // Here we set Z desired to 1 meter, and (x,y)=(0,0) to center the tag in the image + pd.buildFrom(0, 0, 1); + + // Define the task + // - we want an eye-in-hand control law + // - joint velocities are computed + // - interaction matrix is the one at desired position + task.setServo(vpServo::EYEINHAND_L_cVe_eJe); + task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE); + task.set_cVe(cVe); + // We want to see a point on a point + task.addFeature(p, pd); + // Set the gain + //task.setLambda(2.0); + //vpAdaptiveGain lambda(2, 0.7, 30); + vpAdaptiveGain lambda(3.5, 2, 50); + task.setLambda(lambda); + + robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); + + // { + // vpColVector ve(6); + // ve = 0; + // ve[5] = vpMath::rad(5); + // double t_start = vpTime::measureTimeMs(); + // while (vpTime::measureTimeMs() - t_start < 3000) { + // robot.get_eJe(eJe); + // vpColVector q_dot = (cVe * eJe).pseudoInverse() * ve; + // robot.setVelocity(vpRobot::JOINT_STATE, q_dot); + // vpTime::wait(40); + // } + + // return EXIT_SUCCESS; + // } + + + bool quit = false; + bool send_velocities = false; + vpColVector q_dot(robot.getNDof()); + double min_pix_error = 10; // In pixels + double min_error = vpMath::sqr(min_pix_error / cam.get_px()); + + while (!quit) { + g.acquire(I); + vpDisplay::display(I); + + { + std::stringstream ss; + ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit."; + vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red); + } + + if (detector.detect(I)) { + // We consider the first tag only + vpImagePoint cog = detector.getCog(0); // 0 is the id of the first tag + + vpFeatureBuilder::create(p, cam, cog); + + // Get the jacobian + robot.get_eJe(eJe); + task.set_eJe(eJe); + + q_dot = task.computeControlLaw(); + + vpServoDisplay::display(task, cam, I); + vpDisplay::flush(I); + + double error = (task.getError()).sumSquare(); + if (opt_verbose) { + std::cout << "|| s - s* || = " << error << std::endl; + } + if (error < min_error) { + if (opt_verbose) { + std::cout << "Stop the robot" << std::endl; + } + q_dot = 0; + } + } + else { + q_dot = 0; + } + if (!send_velocities) { + q_dot = 0; + } + + robot.setVelocity(vpRobot::JOINT_STATE, q_dot); + + vpDisplay::flush(I); + + vpMouseButton::vpMouseButtonType button; + if (vpDisplay::getClick(I, button, false)) { + switch (button) { + case vpMouseButton::button1: + send_velocities = !send_velocities; + break; + + case vpMouseButton::button3: + quit = true; + q_dot = 0; + break; + + default: + break; + } + } + } + + std::cout << "Stop the robot " << std::endl; + robot.setRobotState(vpRobot::STATE_STOP); + + return EXIT_SUCCESS; + } + catch (const vpException &e) { + std::cout << "Catch an exception: " << e.getMessage() << std::endl; + return EXIT_FAILURE; + } +} + +#else +int main() +{ + std::cout << "You do not have a Pololu PTU connected to your computer..." << std::endl; + return EXIT_SUCCESS; +} +#endif diff --git a/modules/core/include/visp3/core/vpCannyEdgeDetection.h b/modules/core/include/visp3/core/vpCannyEdgeDetection.h index 18f4920c5b..4bc4ebedfa 100644 --- a/modules/core/include/visp3/core/vpCannyEdgeDetection.h +++ b/modules/core/include/visp3/core/vpCannyEdgeDetection.h @@ -116,8 +116,10 @@ class VISP_EXPORT vpCannyEdgeDetection * \brief Step 3: Edge thining. * \details Perform the edge thining step. * Perform a non-maximum suppression to keep only local maxima as edge candidates. + * \param[in] lowerThreshold Edge candidates that are below this threshold are definitely not + * edges. */ - void performEdgeThining(); + void performEdgeThinning(const float &lowerThreshold); /** * \brief Perform hysteresis thresholding. diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h index 2e1327b895..d784891c53 100644 --- a/modules/core/include/visp3/core/vpDisplay.h +++ b/modules/core/include/visp3/core/vpDisplay.h @@ -45,6 +45,7 @@ #include #include #include +#include #include /*! @@ -284,18 +285,6 @@ class VISP_EXPORT vpDisplay */ virtual void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1) = 0; - /*! - * Display a string at the image point \e ip location. - * - * To select the font used to display the string, use setFont(). - * - * \param ip : Upper left image point location of the string in the display. - * \param text : String to display in overlay. - * \param color : String color. - * - * \sa setFont() - */ - virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green) = 0; /*! * Display a circle. @@ -424,6 +413,19 @@ class VISP_EXPORT vpDisplay virtual void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1) = 0; + /*! + * Display a string at the image point \e ip location. + * + * To select the font used to display the string, use setFont(). + * + * \param ip : Upper left image point location of the string in the display. + * \param text : String to display in overlay. + * \param color : String color. + * + * \sa setFont() + */ + virtual void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color = vpColor::green) = 0; + /*! * Flushes the display. * It's necessary to use this function to see the results of any drawing. @@ -671,7 +673,7 @@ class VISP_EXPORT vpDisplay /*! * Set the font used to display a text in overlay. The display is - * performed using displayCharString(). + * performed using displayText(). * * \param font : The expected font name. The available fonts are given by * the "xlsfonts" binary. To choose a font you can also use the @@ -680,7 +682,7 @@ class VISP_EXPORT vpDisplay * \note Under UNIX, to know all the available fonts, use the * "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary. * - * \sa displayCharString() + * \sa displayText() */ virtual void setFont(const std::string &font) = 0; /*! @@ -713,10 +715,6 @@ class VISP_EXPORT vpDisplay unsigned int thickness = 1); static void displayCamera(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness); - static void displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, - const vpColor &color); - static void displayCharString(const vpImage &I, int i, int j, const char *string, - const vpColor &color); static void displayCircle(const vpImage &I, const vpImageCircle &circle, const vpColor &color, bool fill = false, unsigned int thickness = 1); static void displayCircle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, @@ -761,6 +759,8 @@ class VISP_EXPORT vpDisplay unsigned int thickness = 1); static void displayPolygon(const vpImage &I, const std::vector &vip, const vpColor &color, unsigned int thickness = 1, bool closed = true); + static void displayPolygon(const vpImage &I, const vpPolygon &polygon, + const vpColor &color, unsigned int thickness = 1, bool closed = true); static void displayRectangle(const vpImage &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1); @@ -822,11 +822,8 @@ class VISP_EXPORT vpDisplay unsigned int thickness = 1); static void displayCamera(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness); - static void displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, - const vpColor &color); - static void displayCharString(const vpImage &I, int i, int j, const char *string, const vpColor &color); static void displayCircle(const vpImage &I, const vpImageCircle &circle, - const vpColor &color, bool fill = false, unsigned int thickness = 1); + const vpColor &color, bool fill = false, unsigned int thickness = 1); static void displayCircle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1); static void displayCircle(const vpImage &I, int i, int j, unsigned int radius, const vpColor &color, @@ -867,6 +864,8 @@ class VISP_EXPORT vpDisplay static void displayPoint(const vpImage &I, int i, int j, const vpColor &color, unsigned int thickness = 1); static void displayPolygon(const vpImage &I, const std::vector &vip, const vpColor &color, unsigned int thickness = 1, bool closed = true); + static void displayPolygon(const vpImage &I, const vpPolygon &polygon, + const vpColor &color, unsigned int thickness = 1, bool closed = true); static void displayRectangle(const vpImage &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1); @@ -910,6 +909,22 @@ class VISP_EXPORT vpDisplay static void setWindowPosition(const vpImage &I, int winx, int winy); //@} +#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) + /*! + * @name Deprecated functions + */ + //@{ + vp_deprecated static void displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, + const vpColor &color); + vp_deprecated static void displayCharString(const vpImage &I, int i, int j, const char *string, + const vpColor &color); + vp_deprecated static void displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, + const vpColor &color); + vp_deprecated static void displayCharString(const vpImage &I, int i, int j, const char *string, + const vpColor &color); + //@} +#endif + private: //! Get the window pixmap and put it in vpRGBa image. virtual void getImage(vpImage &I) = 0; diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h index 4b0b40e011..3b8df8e65b 100644 --- a/modules/core/include/visp3/core/vpIoTools.h +++ b/modules/core/include/visp3/core/vpIoTools.h @@ -35,9 +35,9 @@ #define _vpIoTools_h_ /*! - \file vpIoTools.h - \brief File and directories basic tools. -*/ + * \file vpIoTools.h + * \brief File and directories basic tools. + */ #include @@ -51,107 +51,6 @@ #include #include -/*! - \class vpIoTools - \ingroup group_core_files_io - \brief File and directories basic tools. - - The example below shows how to manipulate the functions of this - class to create first a directory which name corresponds to the user - name and then create a file in this directory. - - \code -#include -#include -#include -#include - -int main() -{ - std::string username; - vpIoTools::getUserName(username); - - // Test if a username directory exist. If no try to create it - if (vpIoTools::checkDirectory(username) == false) { - try { - // Create a directory with name "username" - vpIoTools::makeDirectory(username); - } - catch (...) { - std::cout << "Cannot create " << username << " directory" << std::endl; - return EXIT_FAILURE; - } - } - // Create a empty filename with name "username/file.txt" - std::ofstream f; - std::string filename = username + "/file.txt"; - // Under Windows converts the filename string into "username\\file.txt" - filename = vpIoTools::path(filename); - std::cout << "Create: " << filename << std::endl; - f.open(filename.c_str()); - f.close(); - - // Rename the file - std::string newfilename = username + "/newfile.txt"; - std::cout << "Rename: " << filename << " in: " << newfilename << std::endl; - if (vpIoTools::rename(filename, newfilename) == false) - std::cout << "Unable to rename: " << filename << std::endl; - - // Remove the file - std::cout << "Remove: " << newfilename << std::endl; - if (vpIoTools::remove(newfilename) == false) - std::cout << "Unable to remove: " << newfilename << std::endl; - - return EXIT_SUCCESS; -} - \endcode - - The example below shows how to read a configuration file and how to create a name - for experiment files. We assume the following file "/home/user/demo/config.txt" : - \code -expNumber 2 -save 0 -lambda 0.4 -use2D 0 -use3D 1 - \endcode - - \code -#include -#include -#include - -int main() -{ - // reading configuration file - vpIoTools::loadConfigFile("/home/user/demo/config.txt"); - std::string nExp;vpIoTools::readConfigVar("expNumber", nExp); // nExp <- "2" - double lambda;vpIoTools::readConfigVar("lambda", lambda); // lambda <- 0.4 - bool use2D;vpIoTools::readConfigVar("use2D", use2D); // use2D <- false - bool use3D;vpIoTools::readConfigVar("use3D", use3D); // use3D <- true - bool doSave;vpIoTools::readConfigVar("save", doSave); // doSave <- false - - // creating name for experiment files - vpIoTools::setBaseDir("/home/user/data"); - // full name <- "/home/user/data/exp2" - vpIoTools::setBaseName("exp" + nExp); - // full name <- "/home/user/data/exp2" since use2D==false - vpIoTools::addNameElement("2D", use2D); - // full name <- "/home/user/data/exp2_3D" - vpIoTools::addNameElement("3D", use3D); - // full name <- "/home/user/data/exp2_3D_lambda0.4" - vpIoTools::addNameElement("lambda", lambda); - - // Saving file.Would copy "/home/user/demo/config.txt" to - // "/home/user/data/exp2_3D_lambda0.4_config.txt" if doSave was true - vpIoTools::saveConfigFile(doSave); - // create sub directory - vpIoTools::createBaseNamePath(); // creates "/home/user/data/exp2_3D_lambda0.4/" -} - \endcode - - */ - // TODO: // Copyright (C) 2011 Carl Rogers // Released under MIT License @@ -434,9 +333,108 @@ template EXPORT std::vector create_npy_header(const std::vecto return header; } -} -} +} // namespace cnpy +} // namespace visp +/*! + * \class vpIoTools + * \ingroup group_core_files_io + * \brief File and directories basic tools. + * + * The example below shows how to manipulate the functions of this + * class to create first a directory which name corresponds to the user + * name and then create a file in this directory. + * + * \code + * #include + * #include + * #include + * #include + * + * int main() + * { + * std::string username; + * vpIoTools::getUserName(username); + * + * // Test if a username directory exist. If no try to create it + * if (vpIoTools::checkDirectory(username) == false) { + * try { + * // Create a directory with name "username" + * vpIoTools::makeDirectory(username); + * } + * catch (...) { + * std::cout << "Cannot create " << username << " directory" << std::endl; + * return EXIT_FAILURE; + * } + * } + * // Create a empty filename with name "username/file.txt" + * std::ofstream f; + * std::string filename = username + "/file.txt"; + * // Under Windows converts the filename string into "username\\file.txt" + * filename = vpIoTools::path(filename); + * std::cout << "Create: " << filename << std::endl; + * f.open(filename.c_str()); + * f.close(); + * + * // Rename the file + * std::string newfilename = username + "/newfile.txt"; + * std::cout << "Rename: " << filename << " in: " << newfilename << std::endl; + * if (vpIoTools::rename(filename, newfilename) == false) + * std::cout << "Unable to rename: " << filename << std::endl; + * + * // Remove the file + * std::cout << "Remove: " << newfilename << std::endl; + * if (vpIoTools::remove(newfilename) == false) + * std::cout << "Unable to remove: " << newfilename << std::endl; + * + * return EXIT_SUCCESS; + * } + * \endcode + * + * The example below shows how to read a configuration file and how to create a name + * for experiment files. We assume the following file "/home/user/demo/config.txt" : + * \code + * expNumber 2 + * save 0 + * lambda 0.4 + * use2D 0 + * use3D 1 + * \endcode + * + * \code + * #include + * #include + * #include + * + * int main() + * { + * // reading configuration file + * vpIoTools::loadConfigFile("/home/user/demo/config.txt"); + * std::string nExp;vpIoTools::readConfigVar("expNumber", nExp); // nExp <- "2" + * double lambda;vpIoTools::readConfigVar("lambda", lambda); // lambda <- 0.4 + * bool use2D;vpIoTools::readConfigVar("use2D", use2D); // use2D <- false + * bool use3D;vpIoTools::readConfigVar("use3D", use3D); // use3D <- true + * bool doSave;vpIoTools::readConfigVar("save", doSave); // doSave <- false + * + * // creating name for experiment files + * vpIoTools::setBaseDir("/home/user/data"); + * // full name <- "/home/user/data/exp2" + * vpIoTools::setBaseName("exp" + nExp); + * // full name <- "/home/user/data/exp2" since use2D==false + * vpIoTools::addNameElement("2D", use2D); + * // full name <- "/home/user/data/exp2_3D" + * vpIoTools::addNameElement("3D", use3D); + * // full name <- "/home/user/data/exp2_3D_lambda0.4" + * vpIoTools::addNameElement("lambda", lambda); + * + * // Saving file.Would copy "/home/user/demo/config.txt" to + * // "/home/user/data/exp2_3D_lambda0.4_config.txt" if doSave was true + * vpIoTools::saveConfigFile(doSave); + * // create sub directory + * vpIoTools::createBaseNamePath(); // creates "/home/user/data/exp2_3D_lambda0.4/" + * } + * \endcode + */ class VISP_EXPORT vpIoTools { @@ -462,15 +460,11 @@ class VISP_EXPORT vpIoTools static bool rename(const std::string &oldfilename, const std::string &newfilename); /*! - Define the directory separator character, backslash ('\') for windows - platform or slash ('/') otherwise. + * Define the directory separator character, backslash ('\') for windows + * platform or slash ('/') otherwise. */ - static const char separator = -#if defined(_WIN32) - '\\'; -#else - '/'; -#endif + static const char separator; + static std::string toUpperCase(const std::string &input); static std::string toLowerCase(const std::string &input); static std::string getAbsolutePathname(const std::string &pathname); @@ -487,8 +481,8 @@ class VISP_EXPORT vpIoTools static std::vector getDirFiles(const std::string &dirname); /*! - @name Configuration file parsing - */ + * @name Configuration file parsing + */ //@{ // read configuration file static bool loadConfigFile(const std::string &confFile); diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h index c321b18266..19fdade01f 100644 --- a/modules/core/include/visp3/core/vpMath.h +++ b/modules/core/include/visp3/core/vpMath.h @@ -32,10 +32,10 @@ */ /*! - \file vpMath.h - \brief Provides simple Math computation that are not available in - the C mathematics library (math.h) -*/ + * \file vpMath.h + * \brief Provides simple Math computation that are not available in + * the C mathematics library (math.h) + */ #ifndef vpMATH_HH #define vpMATH_HH @@ -109,21 +109,21 @@ class VISP_EXPORT vpMath { public: /*! - Convert an angle in radians into degrees. - - \param rad : Angle in radians. - \return Angle converted in degrees. - */ + * Convert an angle in radians into degrees. + * + * \param rad : Angle in radians. + * \return Angle converted in degrees. + */ static inline double deg(double rad) { return (rad * 180.0) / M_PI; } static vpColVector deg(const vpRotationVector &r); static vpColVector deg(const vpColVector &r); /*! - Convert an angle in degrees into radian. - \param deg : Angle in degrees. - \return Angle converted in radians. - */ + * Convert an angle in degrees into radian. + * \param deg : Angle in degrees. + * \return Angle converted in radians. + */ static inline double rad(double deg) { return (deg * M_PI) / 180.0; } static vpColVector rad(const vpColVector &r); @@ -215,7 +215,10 @@ class VISP_EXPORT vpMath */ template static inline T clamp(const T &v, const T &lower, const T &upper) { -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + // Check if std:c++17 or higher. + // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) when ViSP + // is used as a 3rdparty. See issue #1274 +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) return std::clamp(v, lower, upper); #else if (upper < lower) { diff --git a/modules/core/include/visp3/core/vpMoment.h b/modules/core/include/visp3/core/vpMoment.h index bfdb378b35..60ccc96ed4 100644 --- a/modules/core/include/visp3/core/vpMoment.h +++ b/modules/core/include/visp3/core/vpMoment.h @@ -49,71 +49,70 @@ class vpMomentDatabase; class vpMomentObject; /*! - \class vpMoment - - \ingroup group_core_moments - - \brief This class defines shared methods/attributes for 2D moments. - - All moments or combination of moments in the moments module are based on - this class. A moment uses a vpMomentObject object to access all useful - information. Moment values are obtained by a 4-step process common for all - moment types: - - Declaration. - \code - vpMoment moment; - \endcode - - Update with object. - \code - moment.update(object); - \endcode - - Compute the moment value - \code - moment.compute(); - \endcode - - Access the values: - \code - std::vector values = moment.get(); - \endcode - - A moment may also be linked to a vpMomentDatabase. Moments linked to a - database are able to access each others values. Some moments can be computed - only if they are linked to a a database containing their dependencies. - Linking to a database is done using the vpMoment::linkTo(...) method. - - There are no constraints about format of the array returned by - vpMoment::get(); any implementation is fine. - - Each moment must have a string name by implementing the char* - vpMoment::name() method which allows to identify the moment in the database. - Each moment must also implement a compute method describing how to obtain - its values from the object. - - \attention Order of moment computation DOES matter: when you compute a - moment using vpMoment::compute(), all moment dependencies must be computed. - We recall that implemented moments are: - - vpMomentAlpha - - vpMomentArea - - vpMomentAreaNormalized - - vpMomentBasic - - vpMomentCentered - - vpMomentCInvariant - - vpMomentGravityCenter - - vpMomentGravityCenterNormalized - -*/ + * \class vpMoment + * + * \ingroup group_core_moments + * + * \brief This class defines shared methods/attributes for 2D moments. + * + * All moments or combination of moments in the moments module are based on + * this class. A moment uses a vpMomentObject object to access all useful + * information. Moment values are obtained by a 4-step process common for all + * moment types: + * - Declaration. + * \code + * vpMoment moment; + * \endcode + * - Update with object. + * \code + * moment.update(object); + * \endcode + * - Compute the moment value + * \code + * moment.compute(); + * \endcode + * - Access the values: + * \code + * std::vector values = moment.get(); + * \endcode + * + * A moment may also be linked to a vpMomentDatabase. Moments linked to a + * database are able to access each others values. Some moments can be computed + * only if they are linked to a a database containing their dependencies. + * Linking to a database is done using the vpMoment::linkTo(...) method. + * + * There are no constraints about format of the array returned by + * vpMoment::get(); any implementation is fine. + * + * Each moment must have a string name by implementing the char* + * vpMoment::name() method which allows to identify the moment in the database. + * Each moment must also implement a compute method describing how to obtain + * its values from the object. + * + * \attention Order of moment computation DOES matter: when you compute a + * moment using vpMoment::compute(), all moment dependencies must be computed. + * We recall that implemented moments are: + * - vpMomentAlpha + * - vpMomentArea + * - vpMomentAreaNormalized + * - vpMomentBasic + * - vpMomentCentered + * - vpMomentCInvariant + * - vpMomentGravityCenter + * - vpMomentGravityCenterNormalized + */ class VISP_EXPORT vpMoment { private: vpMomentObject *object; vpMomentDatabase *moments; - char _name[255]; + std::string m_name; protected: std::vector values; /*! - Returns the linked moment database. - \return the moment database + * Returns the linked moment database. + * \return the moment database */ inline vpMomentDatabase &getMoments() const { return *moments; } @@ -137,19 +136,19 @@ class VISP_EXPORT vpMoment /*! Virtual destructor. */ - virtual ~vpMoment() {} + virtual ~vpMoment() { } /** @name Inherited functionalities from vpMoment */ //@{ virtual void compute() = 0; inline const vpMomentObject &getObject() const { return *object; } /*! - Returns all values computed by the moment. - \return vector of values + * Returns all values computed by the moment. + * \return vector of values */ const std::vector &get() const { return values; } void linkTo(vpMomentDatabase &moments); - virtual const char *name() const = 0; + virtual const std::string name() const = 0; virtual void printDependencies(std::ostream &os) const; void update(vpMomentObject &object); //@} diff --git a/modules/core/include/visp3/core/vpMomentAlpha.h b/modules/core/include/visp3/core/vpMomentAlpha.h index 56eada8a9c..1e0a991482 100644 --- a/modules/core/include/visp3/core/vpMomentAlpha.h +++ b/modules/core/include/visp3/core/vpMomentAlpha.h @@ -220,7 +220,7 @@ class VISP_EXPORT vpMomentAlpha : public vpMoment /*! * Moment name. */ - const char *name() const { return "vpMomentAlpha"; } + const std::string name() const { return "vpMomentAlpha"; } /*! * Returns true if the alpha moment was constructed as a reference with values in \f$ [-\pi/2 ; \pi/2] \f$, false diff --git a/modules/core/include/visp3/core/vpMomentArea.h b/modules/core/include/visp3/core/vpMomentArea.h index 9a05d6f8b7..ccef048ad6 100644 --- a/modules/core/include/visp3/core/vpMomentArea.h +++ b/modules/core/include/visp3/core/vpMomentArea.h @@ -62,7 +62,7 @@ class VISP_EXPORT vpMomentArea : public vpMoment //@{ void compute(); //! Moment name. - const char *name() const { return "vpMomentArea"; } + const std::string name() const { return "vpMomentArea"; } void printDependencies(std::ostream &os) const; //@} friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentArea &m); diff --git a/modules/core/include/visp3/core/vpMomentAreaNormalized.h b/modules/core/include/visp3/core/vpMomentAreaNormalized.h index 342bbf6c83..212ad34ffe 100644 --- a/modules/core/include/visp3/core/vpMomentAreaNormalized.h +++ b/modules/core/include/visp3/core/vpMomentAreaNormalized.h @@ -177,7 +177,7 @@ class VISP_EXPORT vpMomentAreaNormalized : public vpMoment /*! * Moment name. */ - const char *name() const { return "vpMomentAreaNormalized"; } + const std::string name() const { return "vpMomentAreaNormalized"; } friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAreaNormalized &v); void printDependencies(std::ostream &os) const; }; diff --git a/modules/core/include/visp3/core/vpMomentBasic.h b/modules/core/include/visp3/core/vpMomentBasic.h index 7597478402..e0bc51abb2 100644 --- a/modules/core/include/visp3/core/vpMomentBasic.h +++ b/modules/core/include/visp3/core/vpMomentBasic.h @@ -77,7 +77,7 @@ class VISP_EXPORT vpMomentBasic : public vpMoment /*! Moment name. */ - const char *name() const { return "vpMomentBasic"; } + const std::string name() const { return "vpMomentBasic"; } friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentBasic &v); void printDependencies(std::ostream &os) const; }; diff --git a/modules/core/include/visp3/core/vpMomentCInvariant.h b/modules/core/include/visp3/core/vpMomentCInvariant.h index 106d576f61..58290fd9e4 100644 --- a/modules/core/include/visp3/core/vpMomentCInvariant.h +++ b/modules/core/include/visp3/core/vpMomentCInvariant.h @@ -223,7 +223,7 @@ class VISP_EXPORT vpMomentCInvariant : public vpMoment /*! Moment name. */ - const char *name() const { return "vpMomentCInvariant"; } + const std::string name() const { return "vpMomentCInvariant"; } /*! Print partial invariant. diff --git a/modules/core/include/visp3/core/vpMomentCentered.h b/modules/core/include/visp3/core/vpMomentCentered.h index 90b0a00f78..0f6afae38c 100644 --- a/modules/core/include/visp3/core/vpMomentCentered.h +++ b/modules/core/include/visp3/core/vpMomentCentered.h @@ -83,7 +83,7 @@ class VISP_EXPORT vpMomentCentered : public vpMoment /*! Moment name. */ - inline const char *name() const { return "vpMomentCentered"; } + inline const std::string name() const { return "vpMomentCentered"; } friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCentered &v); void printWithIndices(std::ostream &os) const; diff --git a/modules/core/include/visp3/core/vpMomentDatabase.h b/modules/core/include/visp3/core/vpMomentDatabase.h index bb66dc2525..0cd4d7cee1 100644 --- a/modules/core/include/visp3/core/vpMomentDatabase.h +++ b/modules/core/include/visp3/core/vpMomentDatabase.h @@ -30,10 +30,11 @@ * Description: * Pseudo-database used to handle dependencies between moments */ + /*! - \file vpMomentDatabase.h - \brief Pseudo-database used to handle dependencies between moments. -*/ + * \file vpMomentDatabase.h + * \brief Pseudo-database used to handle dependencies between moments. + */ #ifndef _vpMomentDatabase_h_ #define _vpMomentDatabase_h_ @@ -47,93 +48,93 @@ class vpMoment; class vpMomentObject; /*! - \class vpMomentDatabase - - \ingroup group_core_moments - - \brief This class allows to register all vpMoments so they can access each - other according to their dependencies. - - Sometimes, a moment needs to have access to other moment's values to be - computed. For example vpMomentCentered needs additional information about the - gravity center vpMomentGravityCenter in order to compute the moment's value - from a vpMomentObject. This gravity center should be stored in a - vpMomentDatabase where it can be accessed. - - All moments in a database can access each other freely at any time. They can - also verify if a moment is present in the database or not. Here is a example - of a dependency between two moments using a vpMomentDatabase: - - \code -#include -#include -#include -#include -#include -#include - -int main() -{ - vpPoint p; - std::vector vec_p; // vector that contains the vertices of the contour polygon - - p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1) - vec_p.push_back(p); - p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2) - vec_p.push_back(p); - vpMomentObject obj(1); // Create an image moment object with 1 as - // maximum order (sufficient for gravity center) - obj.setType(vpMomentObject::DISCRETE); // The object is defined by - // two discrete points - obj.fromVector(vec_p); // Init the dense object with the polygon - - vpMomentDatabase db; - vpMomentGravityCenter g; // declaration of gravity center - vpMomentCentered mc; // mc contains centered moments - - g.linkTo(db); //add gravity center to database - mc.linkTo(db); //centered moments depend on gravity, add them to the - //database to grant access - - db.updateAll(obj); // All of the moments must be updated, not just mc - - //There is no global compute method since the order of compute calls - //depends on the implementation - g.compute(); // compute the moment - mc.compute(); //compute centered moments AFTER gravity center - - std::cout << "Gravity center: " << g << std:: endl; // print gravity center moment - std::cout << "Centered moments: " << mc << std:: endl; // print centered moment - - return 0; -} - \endcode - - The following code outputs: - \code -Gravity center: -Xg=1.5, Yg=1.5 -Centered moments: -2 0 -0 x - \endcode - - A moment is identified in the database by it's vpMoment::name method. -Consequently, a database can contain at most one moment of each type. Often it -is useful to update all moments with the same object. Shortcuts -(vpMomentDatabase::updateAll) are provided for that matter. -*/ + * \class vpMomentDatabase + * + * \ingroup group_core_moments + * + * \brief This class allows to register all vpMoments so they can access each + * other according to their dependencies. + * + * Sometimes, a moment needs to have access to other moment's values to be + * computed. For example vpMomentCentered needs additional information about the + * gravity center vpMomentGravityCenter in order to compute the moment's value + * from a vpMomentObject. This gravity center should be stored in a + * vpMomentDatabase where it can be accessed. + * + * All moments in a database can access each other freely at any time. They can + * also verify if a moment is present in the database or not. Here is a example + * of a dependency between two moments using a vpMomentDatabase: + * + * \code + * #include + * #include + * #include + * #include + * #include + * #include + * + * int main() + * { + * vpPoint p; + * std::vector vec_p; // vector that contains the vertices of the contour polygon + * + * p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1) + * vec_p.push_back(p); + * p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2) + * vec_p.push_back(p); + * vpMomentObject obj(1); // Create an image moment object with 1 as + * // maximum order (sufficient for gravity center) + * obj.setType(vpMomentObject::DISCRETE); // The object is defined by + * // two discrete points + * obj.fromVector(vec_p); // Init the dense object with the polygon + * + * vpMomentDatabase db; + * vpMomentGravityCenter g; // declaration of gravity center + * vpMomentCentered mc; // mc contains centered moments + * + * g.linkTo(db); //add gravity center to database + * mc.linkTo(db); //centered moments depend on gravity, add them to the + * //database to grant access + * + * db.updateAll(obj); // All of the moments must be updated, not just mc + * + * //There is no global compute method since the order of compute calls + * //depends on the implementation + * g.compute(); // compute the moment + * mc.compute(); //compute centered moments AFTER gravity center + * + * std::cout << "Gravity center: " << g << std:: endl; // print gravity center moment + * std::cout << "Centered moments: " << mc << std:: endl; // print centered moment + * + * return 0; + * } + * \endcode + * + * The following code outputs: + * \code + * Gravity center: + * Xg=1.5, Yg=1.5 + * Centered moments: + * 2 0 + * 0 x + * \endcode + * + * A moment is identified in the database by it's vpMoment::name method. + * Consequently, a database can contain at most one moment of each type. Often it + * is useful to update all moments with the same object. Shortcuts + * (vpMomentDatabase::updateAll) are provided for that matter. + */ class VISP_EXPORT vpMomentDatabase { private: #ifndef DOXYGEN_SHOULD_SKIP_THIS struct vpCmpStr_t { - bool operator()(char const *a, char const *b) const { return std::strcmp(a, b) < 0; } + bool operator()(const std::string &a, const std::string &b) const { return std::strcmp(a.c_str(), b.c_str()) < 0; } }; #endif - std::map moments; - void add(vpMoment &moment, const char *name); + std::map moments; + void add(vpMoment &moment, const std::string &name); public: vpMomentDatabase() : moments() { } @@ -141,12 +142,14 @@ class VISP_EXPORT vpMomentDatabase /** @name Inherited functionalities from vpMomentDatabase */ //@{ - const vpMoment &get(const char *type, bool &found) const; + const vpMoment &get(const std::string &moment_name, bool &found) const; /*! - Get the first element in the database. - May be useful in case an unnamed object is present but is the only element - in the database. \return the first element in the database. - */ + * Get the first element in the database. + * May be useful in case an unnamed object is present but is the only element + * in the database. + * + * \return the first element in the database. + */ vpMoment &get_first() { return *(moments.begin()->second); } virtual void updateAll(vpMomentObject &object); diff --git a/modules/core/include/visp3/core/vpMomentGravityCenter.h b/modules/core/include/visp3/core/vpMomentGravityCenter.h index 39171649ff..83e61ecfb2 100644 --- a/modules/core/include/visp3/core/vpMomentGravityCenter.h +++ b/modules/core/include/visp3/core/vpMomentGravityCenter.h @@ -31,10 +31,10 @@ * 2D Gravity Center moment descriptor (usually described by the pair Xg,Yg) */ /*! - \file vpMomentGravityCenter.h - \brief 2D Gravity Center moment descriptor (usually described by the pair - Xg,Yg). -*/ + * \file vpMomentGravityCenter.h + * \brief 2D Gravity Center moment descriptor (usually described by the pair + * Xg,Yg). + */ #ifndef _vpMomentGravityCenter_h_ #define _vpMomentGravityCenter_h_ @@ -43,67 +43,67 @@ class vpMomentObject; /*! - \class vpMomentGravityCenter - - \ingroup group_core_moments - - \brief Class describing 2D gravity center moment. - - This moment can be computed from scratch (no need to compute any different - moments before computing this). It gives access to both coordinates of the - gravity center \f$x_g\f$ and \f$y_g\f$. - - These coordinates are defined as follows: \f$x_g = \frac{m_{01}}{m_{00}} - \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$ - \code -#include -#include -#include -#include - -int main() -{ - // Define the contour of an object by a 5 clockwise vertices on a plane - vpPoint p; - std::vector vec_p; // vector that contains the vertices of the contour polygon - - p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1) - vec_p.push_back(p); - p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2) - vec_p.push_back(p); - p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3) - vec_p.push_back(p); - p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4) - vec_p.push_back(p); - p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1) - vec_p.push_back(p); - - vpMomentObject obj(1); // Create an image moment object with 1 as - // maximum order (because only m00,m01,m10 - // are needed to compute the gravity center primitive. - obj.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon - obj.fromVector(vec_p); // Init the dense object with the polygon - - vpMomentGravityCenter g; // declaration of gravity center - g.update(obj); // specify the object - g.compute(); // compute the moment - - std::cout << "Xg=" << g.getXg() << std::endl; // access to Xg - std::cout << "Yg=" << g.getYg() << std::endl; // access to Yg - - std::cout << g << std:: endl; // print gravity center - - return 0; -} - \endcode - - This example produces the following results: - \code -Xg=0.0166667 -Yg=-0.00833333 -Xg=0.0166667, Yg=-0.00833333 - \endcode -*/ + * \class vpMomentGravityCenter + * + * \ingroup group_core_moments + * + * \brief Class describing 2D gravity center moment. + * + * This moment can be computed from scratch (no need to compute any different + * moments before computing this). It gives access to both coordinates of the + * gravity center \f$x_g\f$ and \f$y_g\f$. + * + * These coordinates are defined as follows: \f$x_g = \frac{m_{01}}{m_{00}} + * \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$ + * \code + * #include + * #include + * #include + * #include + * + * int main() + * { + * // Define the contour of an object by a 5 clockwise vertices on a plane + * vpPoint p; + * std::vector vec_p; // vector that contains the vertices of the contour polygon + * + * p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1) + * vec_p.push_back(p); + * p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2) + * vec_p.push_back(p); + * p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3) + * vec_p.push_back(p); + * p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4) + * vec_p.push_back(p); + * p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1) + * vec_p.push_back(p); + * + * vpMomentObject obj(1); // Create an image moment object with 1 as + * // maximum order (because only m00,m01,m10 + * // are needed to compute the gravity center primitive. + * obj.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a contour polygon + * obj.fromVector(vec_p); // Init the dense object with the polygon + * + * vpMomentGravityCenter g; // declaration of gravity center + * g.update(obj); // specify the object + * g.compute(); // compute the moment + * + * std::cout << "Xg=" << g.getXg() << std::endl; // access to Xg + * std::cout << "Yg=" << g.getYg() << std::endl; // access to Yg + * + * std::cout << g << std:: endl; // print gravity center + * + * return 0; + * } + * \endcode + * + * This example produces the following results: + * \code + * Xg=0.0166667 + * Yg=-0.00833333 + * Xg=0.0166667, Yg=-0.00833333 + * \endcode + */ class VISP_EXPORT vpMomentGravityCenter : public vpMoment { @@ -115,19 +115,19 @@ class VISP_EXPORT vpMomentGravityCenter : public vpMoment void compute(); const std::vector &get() const; /*! - Shortcut function to retrieve \f$x_g\f$. - \return The first gravity center coordinate. + * Shortcut function to retrieve \f$x_g\f$. + * \return The first gravity center coordinate. */ double getXg() const { return values[0]; } /*! - Shortcut function to retrieve \f$y_g\f$. - \return The second gravity center coordinate. + * Shortcut function to retrieve \f$y_g\f$. + * \return The second gravity center coordinate. */ double getYg() const { return values[1]; } /*! - The class's string name. + * The class's string name. */ - const char *name() const { return "vpMomentGravityCenter"; } + const std::string name() const { return "vpMomentGravityCenter"; } void printDependencies(std::ostream &os) const; //@} friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentGravityCenter &v); diff --git a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h index aeb9d02b5b..6523a74a17 100644 --- a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h +++ b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h @@ -44,26 +44,26 @@ class vpMomentObject; /*! - \class vpMomentGravityCenterNormalized - - \ingroup group_core_moments - - \brief Class describing 2D normalized gravity center moment. - - Centered and normalized gravity center moment is defined as follows: - \f$(x_n,y_n)\f$ where \f$x_n = x_g a_n\f$ and \f$y_n = y_g a_n\f$. - - vpMomentGravityCenterNormalized depends on vpMomentAreaNormalized to get - access to \f$a_n\f$ and on vpMomentGravityCenter to get access to - \f$(x_g,y_g)\f$ . -*/ + * \class vpMomentGravityCenterNormalized + * + * \ingroup group_core_moments + * + * \brief Class describing 2D normalized gravity center moment. + * + * Centered and normalized gravity center moment is defined as follows: + * \f$(x_n,y_n)\f$ where \f$x_n = x_g a_n\f$ and \f$y_n = y_g a_n\f$. + * + * vpMomentGravityCenterNormalized depends on vpMomentAreaNormalized to get + * access to \f$a_n\f$ and on vpMomentGravityCenter to get access to + * \f$(x_g,y_g)\f$ . + */ class VISP_EXPORT vpMomentGravityCenterNormalized : public vpMomentGravityCenter { public: vpMomentGravityCenterNormalized(); void compute(); //! Moment name. - const char *name() const { return "vpMomentGravityCenterNormalized"; } + const std::string name() const { return "vpMomentGravityCenterNormalized"; } void printDependencies(std::ostream &os) const; friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentGravityCenterNormalized &v); }; diff --git a/modules/core/include/visp3/core/vpMunkres.h b/modules/core/include/visp3/core/vpMunkres.h index 11db82c4d5..78a35dd10c 100644 --- a/modules/core/include/visp3/core/vpMunkres.h +++ b/modules/core/include/visp3/core/vpMunkres.h @@ -35,11 +35,9 @@ #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) - -// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910] -// Visual Studio: Structured bindings are available from Visual Studio 2017 version 15.3 [1911] +// Check if std:c++17 or higher. +// Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) // System #include @@ -98,8 +96,8 @@ class VISP_EXPORT vpMunkres static STEP_T stepThree(const std::vector > &mask, std::vector &col_cover); template static std::tuple > > - stepFour(const std::vector > &costs, std::vector > &mask, - std::vector &row_cover, std::vector &col_cover); + stepFour(const std::vector > &costs, std::vector > &mask, + std::vector &row_cover, std::vector &col_cover); static STEP_T stepFive(std::vector > &mask, const std::pair &path_0, std::vector &row_cover, std::vector &col_cover); template @@ -107,7 +105,7 @@ class VISP_EXPORT vpMunkres const std::vector &col_cover); private: - static constexpr auto ZeroEpsilon{1e-6}; + static constexpr auto ZeroEpsilon { 1e-6 }; }; enum vpMunkres::ZERO_T : unsigned int { NA = 0, STARRED = 1, PRIMED = 2 }; @@ -267,12 +265,14 @@ vpMunkres::stepFour(const std::vector > &costs, std::vector >(row, col)}; + return { vpMunkres::STEP_T(4), std::nullopt }; // Repeat + } + else { + return { vpMunkres::STEP_T(5), std::make_optional >(row, col) }; } - } else { - return {vpMunkres::STEP_T(6), std::nullopt}; + } + else { + return { vpMunkres::STEP_T(6), std::nullopt }; } } @@ -323,9 +323,9 @@ inline std::vector > vpMunkres::run(std::v auto row_cover = std::vector(sq_size, false); auto col_cover = std::vector(sq_size, false); - std::optional > path_0{std::nullopt}; + std::optional > path_0 { std::nullopt }; - auto step{vpMunkres::STEP_T::ENTRY}; + auto step { vpMunkres::STEP_T::ENTRY }; while (step != vpMunkres::STEP_T::DONE) { switch (step) { case vpMunkres::STEP_T::ENTRY: @@ -357,7 +357,7 @@ inline std::vector > vpMunkres::run(std::v } // Compute the pairs - std::vector > ret{}; + std::vector > ret {}; for (auto i = 0u; i < original_row_size; i++) { if (const auto it = std::find(begin(mask.at(i)), end(mask.at(i)), vpMunkres::ZERO_T::STARRED); it != end(mask.at(i))) { diff --git a/modules/core/src/display/vpDisplay_impl.h b/modules/core/src/display/vpDisplay_impl.h index 33ad7819fa..2fb96b3489 100644 --- a/modules/core/src/display/vpDisplay_impl.h +++ b/modules/core/src/display/vpDisplay_impl.h @@ -103,23 +103,23 @@ void vp_display_display_camera(const vpImage &I, const vpHomogeneousMatrix } template -void vp_display_display_char_string(const vpImage &I, const vpImagePoint &ip, const char *string, +void vp_display_display_text(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color) { if (I.display != nullptr) { - (I.display)->displayCharString(ip, string, color); + (I.display)->displayText(ip, string, color); } } template -void vp_display_display_char_string(const vpImage &I, int i, int j, const char *string, const vpColor &color) +void vp_display_display_text(const vpImage &I, int i, int j, const char *string, const vpColor &color) { if (I.display != nullptr) { vpImagePoint ip; ip.set_i(i); ip.set_j(j); - (I.display)->displayCharString(ip, string, color); + (I.display)->displayText(ip, string, color); } } @@ -456,6 +456,13 @@ void vp_display_display_polygon(const vpImage &I, const std::vector +void vp_display_display_polygon(const vpImage &I, const vpPolygon &polygon, const vpColor &color, + unsigned int thickness, bool closed = true) +{ + vp_display_display_polygon(I, polygon.getCorners(), color, thickness, closed); +} + template void vp_display_display_rectangle(const vpImage &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill, unsigned int thickness) @@ -671,7 +678,7 @@ template void vp_display_display_text(const vpImage &I, const vpImagePoint &ip, const std::string &s, const vpColor &color) { if (I.display != nullptr) { - (I.display)->displayCharString(ip, s.c_str(), color); + (I.display)->displayText(ip, s.c_str(), color); } } @@ -683,7 +690,7 @@ void vp_display_display_text(const vpImage &I, int i, int j, const std::st ip.set_i(i); ip.set_j(j); - (I.display)->displayCharString(ip, s.c_str(), color); + (I.display)->displayText(ip, s.c_str(), color); } } diff --git a/modules/core/src/display/vpDisplay_rgba.cpp b/modules/core/src/display/vpDisplay_rgba.cpp index 1023c614a9..a35cbbfa9a 100644 --- a/modules/core/src/display/vpDisplay_rgba.cpp +++ b/modules/core/src/display/vpDisplay_rgba.cpp @@ -96,8 +96,9 @@ void vpDisplay::displayCamera(const vpImage &I, const vpHomogeneousMatri vp_display_display_camera(I, cMo, cam, size, color, thickness); } +#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! - Display a string at the image point \e ip location. + \deprecated Display a string at the image point \e ip location. Use rather displayText() that does the same. To select the font used to display the string, use setFont(). @@ -112,11 +113,11 @@ void vpDisplay::displayCamera(const vpImage &I, const vpHomogeneousMatri void vpDisplay::displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color) { - vp_display_display_char_string(I, ip, string, color); + vp_display_display_text(I, ip, string, color); } /*! - Display a string at the image point (i,j) location. + \deprecated Display a string at the image point (i,j) location. Use rather displayText() that does the same. To select the font used to display the string, use setFont(). @@ -130,8 +131,9 @@ void vpDisplay::displayCharString(const vpImage &I, const vpImagePoint & */ void vpDisplay::displayCharString(const vpImage &I, int i, int j, const char *string, const vpColor &color) { - vp_display_display_char_string(I, i, j, string, color); + vp_display_display_text(I, i, j, string, color); } +#endif /*! Display a circle. @@ -584,6 +586,20 @@ void vpDisplay::displayPolygon(const vpImage &I, const std::vector &I, const vpPolygon &polygon, const vpColor &color, + unsigned int thickness, bool closed) +{ + vp_display_display_polygon(I, polygon, color, thickness, closed); +} + /*! Display a rectangle with \e topLeft as the top-left corner and \e width and \e height the rectangle size. @@ -1251,7 +1267,7 @@ void vpDisplay::setBackground(const vpImage &I, const vpColor &color) { /*! Set the font of a text printed in the display overlay. To print a - text you may use displayCharString(). + text you may use displayText(). \param I : Image associated to the display window. \param fontname : The expected font name. diff --git a/modules/core/src/display/vpDisplay_uchar.cpp b/modules/core/src/display/vpDisplay_uchar.cpp index af42deedc4..9206db101f 100644 --- a/modules/core/src/display/vpDisplay_uchar.cpp +++ b/modules/core/src/display/vpDisplay_uchar.cpp @@ -96,8 +96,9 @@ void vpDisplay::displayCamera(const vpImage &I, const vpHomogeneo vp_display_display_camera(I, cMo, cam, size, color, thickness); } +#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! - Display a string at the image point \e ip location. + \deprecated Display a string at the image point \e ip location. Use rather displayText() that does the same. To select the font used to display the string, use setFont(). @@ -112,11 +113,11 @@ void vpDisplay::displayCamera(const vpImage &I, const vpHomogeneo void vpDisplay::displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color) { - vp_display_display_char_string(I, ip, string, color); + vp_display_display_text(I, ip, string, color); } /*! - Display a string at the image point (i,j) location. + \deprecated Display a string at the image point (i,j) location. Use rather displayText() that does the same. To select the font used to display the string, use setFont(). @@ -131,8 +132,9 @@ void vpDisplay::displayCharString(const vpImage &I, const vpImage void vpDisplay::displayCharString(const vpImage &I, int i, int j, const char *string, const vpColor &color) { - vp_display_display_char_string(I, i, j, string, color); + vp_display_display_text(I, i, j, string, color); } +#endif /*! Display a circle. @@ -584,6 +586,20 @@ void vpDisplay::displayPolygon(const vpImage &I, const std::vecto vp_display_display_polygon(I, vip, color, thickness, closed); } +/*! + Display a polygon defined by a set of image points. + \param I : The image associated to the display. + \param polygon : Polygon to display. + \param color : Line color. + \param thickness : Line thickness. + \param closed : When true display a closed polygon with a segment between first and last image point. +*/ +void vpDisplay::displayPolygon(const vpImage &I, const vpPolygon &polygon, const vpColor &color, + unsigned int thickness, bool closed) +{ + vp_display_display_polygon(I, polygon, color, thickness, closed); +} + /*! Display a rectangle with \e topLeft as the top-left corner and \e width and \e height the rectangle size. @@ -1256,7 +1272,7 @@ void vpDisplay::setBackground(const vpImage &I, const vpColor &co /*! Set the font of a text printed in the display overlay. To print a - text you may use displayCharString(). + text you may use displayText(). \param I : Image associated to the display window. \param fontname : The expected font name. diff --git a/modules/core/src/image/vpCannyEdgeDetection.cpp b/modules/core/src/image/vpCannyEdgeDetection.cpp index 274ba7f6b6..84f955f491 100644 --- a/modules/core/src/image/vpCannyEdgeDetection.cpp +++ b/modules/core/src/image/vpCannyEdgeDetection.cpp @@ -186,9 +186,6 @@ vpCannyEdgeDetection::detect(const vpImage &I) m_areGradientAvailable = false; // Reset for next call // // Step 3: edge thining - performEdgeThining(); - - // // Step 4: hysteresis thresholding float upperThreshold = m_upperThreshold; float lowerThreshold = m_lowerThreshold; if (upperThreshold < 0) { @@ -200,7 +197,11 @@ vpCannyEdgeDetection::detect(const vpImage &I) // Applying Canny recommendation to have the upper threshold 3 times greater than the lower threshold. lowerThreshold = m_upperThreshold / 3.f; } + // To ensure that if lowerThreshold = 0, we reject null gradient points + lowerThreshold = std::max(lowerThreshold, std::numeric_limits::epsilon()); + performEdgeThinning(lowerThreshold); + // // Step 4: hysteresis thresholding performHysteresisThresholding(lowerThreshold, upperThreshold); // // Step 5: edge tracking @@ -220,65 +221,69 @@ vpCannyEdgeDetection::performFilteringAndGradientComputation(const vpImage(GIx, Iblur, m_fg.data, m_gaussianKernelSize); // Computing the gradients - vpImageFilter::filter(Iblur, m_dIx, m_gradientFilterX); - vpImageFilter::filter(Iblur, m_dIy, m_gradientFilterY); + vpImageFilter::filter(Iblur, m_dIx, m_gradientFilterX, true); + vpImageFilter::filter(Iblur, m_dIy, m_gradientFilterY, true); } else { - std::string errmsg("Currently, the only filtering and gradient operators are Gaussian blur + Sobel"); + std::string errmsg("Currently, the filtering operation \""); + errmsg += vpImageFilter::vpCannyFilteringAndGradientTypeToString(m_filteringAndGradientType); + errmsg += "\" is not handled."; throw(vpException(vpException::notImplementedError, errmsg)); } } /** - * \brief Get the theta quadrant in which lies absoluteTheta and the offset along the horizontal - * and vertical direction where to look for the neighbors. + * \brief Get the interpolation weights and offsets. * * \param[in] absoluteTheta : The absolute value of the angle of the edge, expressed in degrees. - * \param[out] dRowGradPlus : The offset in the vertical positive direction. - * \param[out] dRowGradMinus : The offset in the vertical negative direction. - * \param[out] dColGradPlus : The offset in the horizontal positive direction. - * \param[out] dColGradMinus : The offset in the horizontal negative direction. - * \return The quadrant in which lies the angle of the edge, expressed in degrees. + * \param[out] alpha : The weight of the first point used for the interpolation. + * \param[out] beta : The weight of the second point used for the interpolation. + * \param[out] dRowGradAlpha : The offset along the row attached to the alpha weight. + * \param[out] dRowGradBeta : The offset along the row attached to the beta weight. + * \param[out] dColGradAlpha : The offset along the column attached to the alpha weight. + * \param[out] dColGradBeta : The offset along the column attached to the beta weight. */ -int -getThetaQuadrant(const float &absoluteTheta, int &dRowGradPlus, int &dRowGradMinus, int &dColGradPlus, int &dColGradMinus) +void +getInterpolationWeightsAndOffsets(const float &absoluteTheta, + float &alpha, float &beta, + int &dRowGradAlpha, int &dRowGradBeta, + int &dColGradAlpha, int &dColGradBeta +) { - if (absoluteTheta < 22.5) { - // Angles between -22.5 and 22.5 are mapped to be horizontal axis - dColGradMinus = -1; - dColGradPlus = 1; - dRowGradPlus = dRowGradMinus = 0; - return 0; + float thetaMin = 0.f; + if (absoluteTheta < 45.f) { + // Angles between 0 and 45 deg rely on the horizontal and diagonal points + dColGradAlpha = 1; + dColGradBeta = 1; + dRowGradAlpha = 0; + dRowGradBeta = -1; } - else if (absoluteTheta >= 22.5 && absoluteTheta < 67.5) { - // Angles between 22.5 and 67.5 are mapped to the diagonal 45degree - dRowGradMinus = dColGradMinus = -1; - dRowGradPlus = dColGradPlus = 1; - return 45; + else if (absoluteTheta >= 45.f && absoluteTheta < 90.f) { + // Angles between 45 and 90 deg rely on the diagonal and vertical points + thetaMin = 45.f; + dColGradAlpha = 1; + dColGradBeta = 0; + dRowGradAlpha = -1; + dRowGradBeta = -1; } - else if (absoluteTheta >= 67.5 && absoluteTheta < 112.5) { - // Angles between 67.5 and 112.5 are mapped to the vertical axis - dColGradMinus = dColGradPlus = 0; - dRowGradMinus = -1; - dRowGradPlus = 1; - return 90; + else if (absoluteTheta >= 90.f && absoluteTheta < 135.f) { + // Angles between 90 and 135 deg rely on the vertical and diagonal points + thetaMin = 90.f; + dColGradAlpha = 0; + dColGradBeta = -1; + dRowGradAlpha = -1; + dRowGradBeta = -1; } - else if (absoluteTheta >= 112.5 && absoluteTheta < 157.5) { - // Angles between 112.5 and 157.5 are mapped to the diagonal -45degree - dRowGradMinus = -1; - dColGradMinus = 1; - dRowGradPlus = 1; - dColGradPlus = -1; - return 135; + else if (absoluteTheta >= 135.f && absoluteTheta < 180.f) { + // Angles between 135 and 180 deg rely on the vertical and diagonal points + thetaMin = 135.f; + dColGradAlpha = -1; + dColGradBeta = -1; + dRowGradAlpha = -1; + dRowGradBeta = 0; } - else { - // Angles greater than 157.5 are mapped to be horizontal axis - dColGradMinus = 1; - dColGradPlus = -1; - dRowGradMinus = dRowGradPlus = 0; - return 180; - } - return -1; // Should not reach this point + beta = (absoluteTheta - thetaMin) / 45.f; + alpha = 1.f - beta; } /** @@ -320,123 +325,55 @@ getManhattanGradient(const vpImage &dIx, const vpImage &dIy, const float getAbsoluteTheta(const vpImage &dIx, const vpImage &dIy, const int &row, const int &col) { - float absoluteTheta; + float absoluteTheta = 0.f; float dx = dIx[row][col]; float dy = dIy[row][col]; if (std::abs(dx) < std::numeric_limits::epsilon()) { - absoluteTheta = 90.; + absoluteTheta = 90.f; } else { - absoluteTheta = static_cast(vpMath::deg(std::abs(std::atan(dy / dx)))); + absoluteTheta = static_cast(vpMath::deg(std::abs(std::atan2(dy, dx)))); } return absoluteTheta; } -/** - * \brief Search in the direction of the gradient for the highest value of the gradient. - * - * \param[in] dIx The gradient image along the x-axis. - * \param[in] dIy The gradient image along the y-axis. - * \param[in] row The row of the initial point that is considered. - * \param[in] col The column of the initial point that is considered. - * \param[in] thetaQuadrant The gradient orientation quadrant of the initial point. - * \param[in] dRowGrad The direction of the gradient for the vertical direction. - * \param[in] dColGrad The direction of the gradient for the horizontal direction. - * \param[out] pixelsSeen The list of pixels that are of same gradient orientation quadrant. - * \param[out] bestPixel The pixel having the highest absolute value of gradient. - * \param[out] bestGrad The highest absolute value of gradient. - */ void -searchForBestGradientInGradientDirection(const vpImage &dIx, const vpImage &dIy, -const int &row, const int &col, const int &thetaQuadrant, const int &dRowGrad, const int &dColGrad, -std::vector > &pixelsSeen, std::pair &bestPixel, float &bestGrad) +vpCannyEdgeDetection::performEdgeThinning(const float &lowerThreshold) { - bool isGradientInTheSameDirection = true; - int rowCandidate = row + dRowGrad; - int colCandidate = col + dColGrad; - - while (isGradientInTheSameDirection) { - // Getting the gradients around the edge point - float gradPlus = getManhattanGradient(dIx, dIy, rowCandidate, colCandidate); - if (std::abs(gradPlus) < std::numeric_limits::epsilon()) { - // The gradient is almost null => ignoring the point - isGradientInTheSameDirection = false; - break; - } - int dRowGradPlusCandidate = 0, dRowGradMinusCandidate = 0; - int dColGradPlusCandidate = 0, dColGradMinusCandidate = 0; - float absThetaPlus = getAbsoluteTheta(dIx, dIy, rowCandidate, colCandidate); - int thetaQuadrantCandidate = getThetaQuadrant(absThetaPlus, dRowGradPlusCandidate, dRowGradMinusCandidate, dColGradPlusCandidate, dColGradMinusCandidate); - if (thetaQuadrantCandidate != thetaQuadrant) { - isGradientInTheSameDirection = false; - break; - } - - std::pair pixelCandidate(rowCandidate, colCandidate); - if (gradPlus > bestGrad) { - // The gradient is higher with the next pixel candidate - // Saving it - bestGrad = gradPlus; - pixelsSeen.push_back(bestPixel); - bestPixel = pixelCandidate; - } - else { - // Best pixel is still the best - pixelsSeen.push_back(pixelCandidate); - } - rowCandidate += dRowGrad; - colCandidate += dColGrad; - } -} - -void -vpCannyEdgeDetection::performEdgeThining() -{ - vpImage dIx = m_dIx; - vpImage dIy = m_dIy; int nbRows = m_dIx.getRows(); int nbCols = m_dIx.getCols(); for (int row = 0; row < nbRows; row++) { for (int col = 0; col < nbCols; col++) { // Computing the gradient orientation and magnitude - float grad = getManhattanGradient(dIx, dIy, row, col); + float grad = getManhattanGradient(m_dIx, m_dIy, row, col); - if (grad < std::numeric_limits::epsilon()) { - // The gradient is almost null => ignoring the point + if (grad < lowerThreshold) { + // The gradient is lower than minimum threshold => ignoring the point continue; } - float absoluteTheta = getAbsoluteTheta(dIx, dIy, row, col); - // Getting the offset along the horizontal and vertical axes // depending on the gradient orientation - int dRowGradPlus = 0, dRowGradMinus = 0; - int dColGradPlus = 0, dColGradMinus = 0; - int thetaQuadrant = getThetaQuadrant(absoluteTheta, dRowGradPlus, dRowGradMinus, dColGradPlus, dColGradMinus); - - std::vector > pixelsSeen; - std::pair bestPixel(row, col); - float bestGrad = grad; - - // iterate over all the pixels having the same gradient orientation quadrant - searchForBestGradientInGradientDirection(dIx, dIy, row, col, thetaQuadrant, dRowGradPlus, dColGradPlus, - pixelsSeen, bestPixel, bestGrad); - - searchForBestGradientInGradientDirection(dIx, dIy, row, col, thetaQuadrant, dRowGradMinus, dColGradMinus, - pixelsSeen, bestPixel, bestGrad); - - // Keeping the edge point that has the highest gradient - m_edgeCandidateAndGradient[bestPixel] = bestGrad; - - // Suppressing non-maximum gradient - for (std::vector >::iterator it = pixelsSeen.begin(); it != pixelsSeen.end(); it++) { - // Suppressing non-maximum gradient - int row_temp = it->first; - int col_temp = it->second; - dIx[row_temp][col_temp] = 0.; - dIy[row_temp][col_temp] = 0.; + int dRowAlphaPlus = 0, dRowBetaPlus = 0; + int dColAphaPlus = 0, dColBetaPlus = 0; + float absTheta = getAbsoluteTheta(m_dIx, m_dIy, row, col); + float alpha = 0.f, beta = 0.f; + getInterpolationWeightsAndOffsets(absTheta, alpha, beta, dRowAlphaPlus, dRowBetaPlus, dColAphaPlus, dColBetaPlus); + int dRowAlphaMinus = -dRowAlphaPlus, dRowBetaMinus = -dRowBetaPlus; + int dColAphaMinus = -dColAphaPlus, dColBetaMinus = -dColBetaPlus; + float gradAlphaPlus = getManhattanGradient(m_dIx, m_dIy, row + dRowAlphaPlus, col + dColAphaPlus); + float gradBetaPlus = getManhattanGradient(m_dIx, m_dIy, row + dRowBetaPlus, col + dColBetaPlus); + float gradAlphaMinus = getManhattanGradient(m_dIx, m_dIy, row + dRowAlphaMinus, col + dColAphaMinus); + float gradBetaMinus = getManhattanGradient(m_dIx, m_dIy, row + dRowBetaMinus, col + dColBetaMinus); + float gradPlus = alpha * gradAlphaPlus + beta * gradBetaPlus; + float gradMinus = alpha * gradAlphaMinus + beta * gradBetaMinus; + + if (grad >= gradPlus && grad >= gradMinus) { + // Keeping the edge point that has the highest gradient + std::pair bestPixel(row, col); + m_edgeCandidateAndGradient[bestPixel] = grad; } } } @@ -482,7 +419,9 @@ vpCannyEdgeDetection::recursiveSearchForStrongEdge(const std::pair= nbRows) @@ -501,6 +440,7 @@ vpCannyEdgeDetection::recursiveSearchForStrongEdge(const std::pair -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +// Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) /*! * Find a starred zero in a specific mask matrix row. @@ -54,7 +54,7 @@ std::optional vpMunkres::findStarInRow(const std::vector(std::distance(begin(mask.at(row)), it)) - : std::nullopt; + : std::nullopt; } /*! @@ -84,7 +84,7 @@ std::optional vpMunkres::findPrimeInRow(const std::vector(std::distance(begin(mask.at(row)), it)) - : std::nullopt; + : std::nullopt; } /*! @@ -98,7 +98,7 @@ void vpMunkres::augmentPath(std::vector > &mask, { for (const auto &[row, col] : path) { mask.at(row).at(col) = - mask.at(row).at(col) == vpMunkres::ZERO_T::STARRED ? vpMunkres::ZERO_T::NA : vpMunkres::ZERO_T::STARRED; + mask.at(row).at(col) == vpMunkres::ZERO_T::STARRED ? vpMunkres::ZERO_T::NA : vpMunkres::ZERO_T::STARRED; } } @@ -172,12 +172,13 @@ vpMunkres::STEP_T vpMunkres::stepFive(std::vector const std::pair &path_0, std::vector &row_cover, std::vector &col_cover) { - std::vector > path{path_0}; // Z0 + std::vector > path { path_0 }; // Z0 while (true) { if (const auto star_in_col = findStarInCol(mask, path.back().second)) { path.emplace_back(*star_in_col, path.back().second); // Z1 - } else { + } + else { augmentPath(mask, path); erasePrimes(mask); clearCovers(row_cover, col_cover); @@ -191,4 +192,4 @@ vpMunkres::STEP_T vpMunkres::stepFive(std::vector } } -#endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#endif diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index d0df6586d3..752cfb6e67 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -447,6 +447,13 @@ std::string vpIoTools::configFile = ""; std::vector vpIoTools::configVars = std::vector(); std::vector vpIoTools::configValues = std::vector(); +const char vpIoTools::separator = +#if defined(_WIN32) +'\\'; +#else +'/'; +#endif + namespace { // The following code is not working on iOS since wordexp() is not available diff --git a/modules/core/src/tools/geometry/vpPolygon.cpp b/modules/core/src/tools/geometry/vpPolygon.cpp index ed23cd6642..0cbca328a0 100644 --- a/modules/core/src/tools/geometry/vpPolygon.cpp +++ b/modules/core/src/tools/geometry/vpPolygon.cpp @@ -63,7 +63,8 @@ template std::vector convexHull(const IpCon // Visp -> CV std::vector cv_pts; -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) + // Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) std::transform(cbegin(ips), cend(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) { return cv::Point(static_cast(ip.get_u()), static_cast(ip.get_v())); }); @@ -79,7 +80,8 @@ template std::vector convexHull(const IpCon // CV -> Visp std::vector conv_hull_corners; -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) + // Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) std::transform(cbegin(cv_conv_hull_corners), cend(cv_conv_hull_corners), std::back_inserter(conv_hull_corners), [](const cv::Point &pt) { return vpImagePoint { static_cast(pt.y), static_cast(pt.x) }; diff --git a/modules/core/src/tracking/moments/vpMoment.cpp b/modules/core/src/tracking/moments/vpMoment.cpp index dced3cf428..121ab5dc81 100644 --- a/modules/core/src/tracking/moments/vpMoment.cpp +++ b/modules/core/src/tracking/moments/vpMoment.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,95 +29,89 @@ * * Description: * Base for 2D moment descriptor - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! - \file vpMoment.cpp - \brief Base class for all 2D moments. -*/ + * \file vpMoment.cpp + * \brief Base class for all 2D moments. + */ #include #include #include #include /*! - Default constructor -*/ -vpMoment::vpMoment() : object(nullptr), moments(nullptr), values() {} + * Default constructor + */ +vpMoment::vpMoment() : object(nullptr), moments(nullptr), values() { } /*! - Links the moment to a database of moment primitives. - If the moment depends on other moments, these moments must be linked to the -same database. \attention Two moments of the same class cannot be stored in -the same database -\code -#include -#include -#include -#include -#include - -int main() -{ - vpPoint p; - std::vector vec_p; - - p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1) - vec_p.push_back(p); p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2) - - vpMomentObject obj(2); - obj.setType(vpMomentObject::DISCRETE); // Discrete mode. - obj.fromVector(vec_p); // Init the dense object with the polygon - - vpMomentDatabase db; - vpMomentGravityCenter G; // declaration of gravity center - vpMomentCentered mc; // mc contains centered moments - - G.linkTo(db); //add gravity center to database - mc.linkTo(db); //centered moments depend on gravity, add them to the - //database to grant access - - G.update(obj); // specify the object for gravity center - mc.update(obj); // and for centered moments - - G.compute(); // compute the moment - mc.compute(); //compute centered moments AFTER gravity center - - return 0; -} - - \endcode - - \param data_base : database of moment primitives. -*/ + * Links the moment to a database of moment primitives. + * If the moment depends on other moments, these moments must be linked to the + * same database. + * \attention Two moments of the same class cannot be stored in + * the same database + * \code + * #include + * #include + * #include + * #include + * #include + * + * int main() + * { + * vpPoint p; + * std::vector vec_p; + * + * p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1) + * vec_p.push_back(p); + * p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2) + * vec_p.push_back(p); + * + * vpMomentObject obj(2); + * obj.setType(vpMomentObject::DISCRETE); // Discrete mode. + * obj.fromVector(vec_p); // Init the dense object with the polygon + * + * vpMomentDatabase db; + * vpMomentGravityCenter G; // declaration of gravity center + * vpMomentCentered mc; // mc contains centered moments + * + * G.linkTo(db); // add gravity center to database + * mc.linkTo(db); // centered moments depend on gravity, add them to the + * // database to grant access + * + * G.update(obj); // specify the object for gravity center + * mc.update(obj); // and for centered moments + * + * G.compute(); // compute the moment + * mc.compute(); // compute centered moments AFTER gravity center + * + * return 0; + * } + * \endcode + * + * \param data_base : database of moment primitives. + */ void vpMoment::linkTo(vpMomentDatabase &data_base) { - if (strlen(name()) >= 255) { - throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the moment name")); - } - - std::strcpy(_name, name()); + m_name = name(); this->moments = &data_base; - data_base.add(*this, _name); + data_base.add(*this, m_name); } /*! - Updates the moment with the current object. This does not compute any - values. \param moment_object : object descriptor of the current camera - vision. -*/ + * Updates the moment with the current object. This does not compute any + * values. + * \param moment_object : object descriptor of the current camera vision. + */ void vpMoment::update(vpMomentObject &moment_object) { this->object = &moment_object; } /*! - Prints the moment contents to a stream - \param os : a std::stream. - \param m : a moment instance. -*/ + * Prints the moment contents to a stream + * \param os : a std::stream. + * \param m : a moment instance. + */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMoment &m) { for (std::vector::const_iterator i = m.values.begin(); i != m.values.end(); ++i) @@ -128,14 +121,14 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMoment &m) } /*! -Prints values of all dependent moments required to calculate a specific -vpMoment. Not made pure to maintain compatibility Recommended : Types -inheriting from vpMoment should implement this function -*/ + * Prints values of all dependent moments required to calculate a specific + * vpMoment. Not made pure to maintain compatibility Recommended : Types + * inheriting from vpMoment should implement this function + */ void vpMoment::printDependencies(std::ostream &os) const { os << " WARNING : Falling back to base class version of " - "printDependencies(). To prevent that, this has to be implemented in " - "the derived classes!" - << std::endl; + "printDependencies(). To prevent that, this has to be implemented in " + "the derived classes!" + << std::endl; } diff --git a/modules/core/src/tracking/moments/vpMomentCommon.cpp b/modules/core/src/tracking/moments/vpMomentCommon.cpp index 93082bcd30..e890a0fb77 100644 --- a/modules/core/src/tracking/moments/vpMomentCommon.cpp +++ b/modules/core/src/tracking/moments/vpMomentCommon.cpp @@ -135,10 +135,9 @@ void vpMomentCommon::updateAll(vpMomentObject &object) momentSurfaceNormalized.compute(); momentGravityNormalized.compute(); momentArea.compute(); - } - catch (const char *ex) { - std::cout << "exception:" << ex << std::endl; + catch (const vpException &e) { + std::cout << "Exception in vpMomentCommon:" << e.getStringMessage() << std::endl; } } diff --git a/modules/core/src/tracking/moments/vpMomentDatabase.cpp b/modules/core/src/tracking/moments/vpMomentDatabase.cpp index 12e28b9d96..aab0306600 100644 --- a/modules/core/src/tracking/moments/vpMomentDatabase.cpp +++ b/modules/core/src/tracking/moments/vpMomentDatabase.cpp @@ -43,56 +43,56 @@ #include /*! - Adds a moment to the database. - \param moment : moment to add - \param : name of the moment's class - - \attention You cannot add two moments with the same name. The rules - for insersion are the same as those of std::map. -*/ -void vpMomentDatabase::add(vpMoment &moment, const char *name) + * Adds a moment to the database. + * \param moment : Moment to add. + * \param name : Name of the moment's class. + * + * \attention You cannot add two moments with the same name. The rules + * for insertion are the same as those of std::map. + */ +void vpMomentDatabase::add(vpMoment &moment, const std::string &name) { - moments.insert(std::pair((const char *)name, &moment)); + moments.insert(std::pair(name, &moment)); } /*! - Retrieves a moment from the database. - \param type : Name of the moment's class. - \param found : true if the moment's type exists in the database, false - otherwise. \return Moment corresponding to \e type. -*/ -const vpMoment &vpMomentDatabase::get(const char *type, bool &found) const + * Retrieves a moment from the database. + * \param moment_name : Name of the moment's class. + * \param found : true if the moment's type exists in the database, false otherwise. + * \return Moment corresponding to \e type. + */ +const vpMoment &vpMomentDatabase::get(const std::string &moment_name, bool &found) const { - std::map::const_iterator it = moments.find(type); + std::map::const_iterator it = moments.find(moment_name); found = (it != moments.end()); return *(it->second); } /*! - Updates the moment object for all moments in the database - \param object : Moment object for which all the moments in the database - should be updated. - - Sometimes, it might be useful to update the whole database when computing - only one moment when this moment depends on other moments. The example - provided in the header of this class gives an example that shows how to - compute gravity center moment and the centered moment using a mass update. -*/ + * Updates the moment object for all moments in the database + * \param object : Moment object for which all the moments in the database + * should be updated. + * + * Sometimes, it might be useful to update the whole database when computing + * only one moment when this moment depends on other moments. The example + * provided in the header of this class gives an example that shows how to + * compute gravity center moment and the centered moment using a mass update. + */ void vpMomentDatabase::updateAll(vpMomentObject &object) { - std::map::const_iterator itr; + std::map::const_iterator itr; for (itr = moments.begin(); itr != moments.end(); ++itr) { (*itr).second->update(object); } } /*! - Outputs all the moments values in the database to a stream. -*/ + * Outputs all the moments values in the database to a stream. + */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentDatabase &m) { - std::map::const_iterator itr; + std::map::const_iterator itr; os << "{"; for (itr = m.moments.begin(); itr != m.moments.end(); ++itr) { diff --git a/modules/core/test/munkres/testMunkres.cpp b/modules/core/test/munkres/testMunkres.cpp index 3b3706cf38..9256e48a21 100644 --- a/modules/core/test/munkres/testMunkres.cpp +++ b/modules/core/test/munkres/testMunkres.cpp @@ -40,8 +40,8 @@ #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +// Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) // System #include @@ -67,21 +67,21 @@ TEST_CASE("Check Munkres-based assignment", "[visp_munkres]") { auto testMunkres = [](const std::vector > &cost_matrix, const std::vector > &expected_pairs) { - const auto munkres_pairs = vpMunkres::run(cost_matrix); - REQUIRE(expected_pairs.size() == munkres_pairs.size()); - for (auto i = 0u; i < munkres_pairs.size(); i++) { - REQUIRE(expected_pairs.at(i) == munkres_pairs.at(i)); - } - }; + const auto munkres_pairs = vpMunkres::run(cost_matrix); + REQUIRE(expected_pairs.size() == munkres_pairs.size()); + for (auto i = 0u; i < munkres_pairs.size(); i++) { + REQUIRE(expected_pairs.at(i) == munkres_pairs.at(i)); + } + }; SECTION("Square cost matrix") { - std::vector > costs{}; + std::vector > costs {}; costs.push_back(std::vector{3, 1, 2}); costs.push_back(std::vector{2, 3, 1}); costs.push_back(std::vector{1, 2, 3}); - std::vector > expected_pairs{}; + std::vector > expected_pairs {}; expected_pairs.emplace_back(0, 1); expected_pairs.emplace_back(1, 2); expected_pairs.emplace_back(2, 0); @@ -91,12 +91,12 @@ TEST_CASE("Check Munkres-based assignment", "[visp_munkres]") SECTION("Horizontal cost matrix") { - std::vector > costs{}; + std::vector > costs {}; costs.push_back(std::vector{4, 1, 2, 3}); costs.push_back(std::vector{3, 4, 1, 2}); costs.push_back(std::vector{2, 3, 4, 1}); - std::vector > expected_pairs{}; + std::vector > expected_pairs {}; expected_pairs.emplace_back(0, 1); expected_pairs.emplace_back(1, 2); expected_pairs.emplace_back(2, 3); @@ -106,13 +106,13 @@ TEST_CASE("Check Munkres-based assignment", "[visp_munkres]") SECTION("Vertical cost matrix") { - std::vector > costs{}; + std::vector > costs {}; costs.push_back(std::vector{4, 1, 2}); costs.push_back(std::vector{3, 4, 1}); costs.push_back(std::vector{2, 3, 4}); costs.push_back(std::vector{1, 2, 3}); - std::vector > expected_pairs{}; + std::vector > expected_pairs {}; expected_pairs.emplace_back(0, 1); expected_pairs.emplace_back(1, 2); expected_pairs.emplace_back(3, 0); @@ -139,9 +139,9 @@ bool testMunkres(const std::vector > &costs_matrix, if (pairs.size() != expected_pairs.size()) { // clang-format off std::cerr << "Expected nb of association | Munkres nb of association: " - << expected_pairs.size() << " | " << pairs.size() - << std::endl; - // clang-format on + << expected_pairs.size() << " | " << pairs.size() + << std::endl; +// clang-format on return false; } @@ -161,8 +161,8 @@ bool testMunkres(const std::vector > &costs_matrix, // Output the pair which fails std::cerr << "FAIL: " - << "Expected association | Munkres association: " << expected_pairs.at(i) << " | " << pairs.at(i) - << std::endl; + << "Expected association | Munkres association: " << expected_pairs.at(i) << " | " << pairs.at(i) + << std::endl; return false; } @@ -173,13 +173,13 @@ bool testMunkres(const std::vector > &costs_matrix, bool testSquareMat() { - std::vector > costs{}; + std::vector > costs {}; costs.push_back(std::vector{3, 4, 1, 2}); costs.push_back(std::vector{3, 4, 2, 1}); costs.push_back(std::vector{1, 2, 3, 4}); costs.push_back(std::vector{2, 1, 4, 3}); - std::vector > pairs{}; + std::vector > pairs {}; pairs.emplace_back(0, 2); pairs.emplace_back(1, 3); pairs.emplace_back(2, 0); @@ -190,13 +190,13 @@ bool testSquareMat() bool testVertMat() { - std::vector > costs{}; + std::vector > costs {}; costs.push_back(std::vector{3, 2, 1}); costs.push_back(std::vector{4, 3, 2}); costs.push_back(std::vector{1, 4, 3}); costs.push_back(std::vector{2, 1, 4}); - std::vector > pairs{}; + std::vector > pairs {}; pairs.emplace_back(0, 2); pairs.emplace_back(2, 0); pairs.emplace_back(3, 1); @@ -206,12 +206,12 @@ bool testVertMat() bool testHorMat() { - std::vector > costs{}; + std::vector > costs {}; costs.push_back(std::vector{2, 3, 4, 1}); costs.push_back(std::vector{4, 1, 2, 3}); costs.push_back(std::vector{1, 2, 3, 4}); - std::vector > pairs{}; + std::vector > pairs {}; pairs.emplace_back(0, 3); pairs.emplace_back(1, 1); pairs.emplace_back(2, 0); diff --git a/modules/core/test/tools/geometry/testPolygon2.cpp b/modules/core/test/tools/geometry/testPolygon2.cpp index 7653e78a15..4625b7b95b 100644 --- a/modules/core/test/tools/geometry/testPolygon2.cpp +++ b/modules/core/test/tools/geometry/testPolygon2.cpp @@ -62,7 +62,8 @@ TEST_CASE("Check OpenCV-bsed convex hull") vpPolygon poly {}; poly.buildFrom(rect_corners, true); -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) + // Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) for (const auto &poly_corner : poly.getCorners()) { REQUIRE(std::find(cbegin(rect_corners), cend(rect_corners), poly_corner) != cend(rect_corners)); } @@ -98,7 +99,8 @@ bool testConvexHull() vpPolygon poly; poly.buildFrom(rect_corners, true); -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) + // Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) for (const auto &poly_corner : poly.getCorners()) { if (std::find(cbegin(rect_corners), cend(rect_corners), poly_corner) == cend(rect_corners)) { return false; diff --git a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h index e440dac32f..92fdda86f2 100644 --- a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h +++ b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h @@ -286,14 +286,22 @@ class VISP_EXPORT vpDetectorAprilTag : public vpDetectorBase void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod); void setAprilTagQuadDecimate(float quadDecimate); void setAprilTagQuadSigma(float quadSigma); - void setAprilTagRefineDecode(bool refineDecode); void setAprilTagRefineEdges(bool refineEdges); - void setAprilTagRefinePose(bool refinePose); +#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! - * Allow to enable the display of overlay tag information in the windows - * (vpDisplay) associated to the input image. + * @name Deprecated functions */ + //@{ + vp_deprecated void setAprilTagRefineDecode(bool refineDecode); + vp_deprecated void setAprilTagRefinePose(bool refinePose); + //@} +#endif + +/*! + * Allow to enable the display of overlay tag information in the windows + * (vpDisplay) associated to the input image. + */ inline void setDisplayTag(bool display, const vpColor &color = vpColor::none, unsigned int thickness = 2) { m_displayTag = display; diff --git a/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h b/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h index 4cdc12fb22..57e955e8e2 100644 --- a/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h +++ b/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h @@ -35,7 +35,11 @@ #include -#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher. +// Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class +#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && \ + ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) + #include #include #include diff --git a/modules/detection/src/dnn/vpDetectorDNNOpenCV.cpp b/modules/detection/src/dnn/vpDetectorDNNOpenCV.cpp index 2d0c1422cc..f8babbc1f4 100644 --- a/modules/detection/src/dnn/vpDetectorDNNOpenCV.cpp +++ b/modules/detection/src/dnn/vpDetectorDNNOpenCV.cpp @@ -34,7 +34,10 @@ *****************************************************************************/ #include -#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && \ + ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) + #include #include #include diff --git a/modules/detection/src/tag/vpDetectorAprilTag.cpp b/modules/detection/src/tag/vpDetectorAprilTag.cpp index b8feb4efdc..0b435a8449 100644 --- a/modules/detection/src/tag/vpDetectorAprilTag.cpp +++ b/modules/detection/src/tag/vpDetectorAprilTag.cpp @@ -66,7 +66,7 @@ class vpDetectorAprilTag::Impl public: Impl(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &method) : m_poseEstimationMethod(method), m_tagsId(), m_tagFamily(tagFamily), m_td(nullptr), m_tf(nullptr), m_detections(nullptr), - m_zAlignedWithCameraFrame(false) + m_zAlignedWithCameraFrame(false) { switch (m_tagFamily) { case TAG_36h11: @@ -135,7 +135,7 @@ class vpDetectorAprilTag::Impl Impl(const Impl &o) : m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagsId(o.m_tagsId), m_tagFamily(o.m_tagFamily), m_td(nullptr), - m_tf(nullptr), m_detections(nullptr), m_zAlignedWithCameraFrame(o.m_zAlignedWithCameraFrame) + m_tf(nullptr), m_detections(nullptr), m_zAlignedWithCameraFrame(o.m_zAlignedWithCameraFrame) { switch (m_tagFamily) { case TAG_36h11: @@ -301,7 +301,7 @@ class vpDetectorAprilTag::Impl if (m_tagFamily == TAG_CIRCLE49h12 || m_tagFamily == TAG_CUSTOM48h12 || m_tagFamily == TAG_STANDARD41h12 || m_tagFamily == TAG_STANDARD52h13) { std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." - << std::endl; + << std::endl; return false; } #endif @@ -309,9 +309,9 @@ class vpDetectorAprilTag::Impl const bool computePose = (cMo_vec != nullptr); image_u8_t im = {/*.width =*/(int32_t)I.getWidth(), - /*.height =*/(int32_t)I.getHeight(), - /*.stride =*/(int32_t)I.getWidth(), - /*.buf =*/I.bitmap}; + /*.height =*/(int32_t)I.getHeight(), + /*.stride =*/(int32_t)I.getWidth(), + /*.buf =*/I.bitmap }; if (m_detections) { apriltag_detections_destroy(m_detections); @@ -458,7 +458,7 @@ class vpDetectorAprilTag::Impl if (m_tagFamily == TAG_CIRCLE49h12 || m_tagFamily == TAG_CUSTOM48h12 || m_tagFamily == TAG_STANDARD41h12 || m_tagFamily == TAG_STANDARD52h13) { std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." - << std::endl; + << std::endl; return false; } #endif @@ -563,7 +563,7 @@ class vpDetectorAprilTag::Impl vpHomogeneousMatrix cMo_dementhon, cMo_lagrange; double residual_dementhon = std::numeric_limits::max(), - residual_lagrange = std::numeric_limits::max(); + residual_lagrange = std::numeric_limits::max(); double residual_homography = pose.computeResidual(cMo_homography); double residual_homography_ortho_iter = pose.computeResidual(cMo_homography_ortho_iter); @@ -588,7 +588,8 @@ class vpDetectorAprilTag::Impl std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin(); cMo = *(poses.begin() + minIndex); - } else { + } + else { pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo); } } @@ -602,16 +603,16 @@ class vpDetectorAprilTag::Impl if (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) { if (cMo2) { double scale = tagSize / 2.0; - double data_p0[] = {-scale, scale, 0}; - double data_p1[] = {scale, scale, 0}; - double data_p2[] = {scale, -scale, 0}; - double data_p3[] = {-scale, -scale, 0}; - matd_t *p[4] = {matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1), - matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3)}; + double data_p0[] = { -scale, scale, 0 }; + double data_p1[] = { scale, scale, 0 }; + double data_p2[] = { scale, -scale, 0 }; + double data_p3[] = { -scale, -scale, 0 }; + matd_t *p[4] = { matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1), + matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3) }; matd_t *v[4]; for (int i = 0; i < 4; i++) { - double data_v[] = {(det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(), - 1}; + double data_v[] = { (det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(), + 1 }; v[i] = matd_create_data(3, 1, data_v); } @@ -680,11 +681,13 @@ class vpDetectorAprilTag::Impl if (cMo2) { if (pose2.R) { convertHomogeneousMatrix(pose2, *cMo2); - } else { + } + else { *cMo2 = cMo1; } } - } else { + } + else { convertHomogeneousMatrix(pose2, cMo1); if (cMo2) { convertHomogeneousMatrix(pose1, *cMo2); @@ -783,7 +786,7 @@ class vpDetectorAprilTag::Impl } } - void setRefineDecode(bool) {} + void setRefineDecode(bool) { } void setRefineEdges(bool refineEdges) { @@ -792,7 +795,7 @@ class vpDetectorAprilTag::Impl } } - void setRefinePose(bool) {} + void setRefinePose(bool) { } void setPoseEstimationMethod(const vpPoseEstimationMethod &method) { m_poseEstimationMethod = method; } @@ -813,17 +816,15 @@ class vpDetectorAprilTag::Impl vpDetectorAprilTag::vpDetectorAprilTag(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &poseEstimationMethod) : m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2), - m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(), - m_impl(new Impl(tagFamily, poseEstimationMethod)) -{ -} + m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(), + m_impl(new Impl(tagFamily, poseEstimationMethod)) +{ } vpDetectorAprilTag::vpDetectorAprilTag(const vpDetectorAprilTag &o) : vpDetectorBase(o), m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2), - m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(), - m_impl(new Impl(*o.m_impl)) -{ -} + m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(), + m_impl(new Impl(*o.m_impl)) +{ } vpDetectorAprilTag &vpDetectorAprilTag::operator=(vpDetectorAprilTag o) { @@ -1026,7 +1027,8 @@ std::vector > vpDetectorAprilTag::getTagsPoints3D(const std throw(vpException(vpException::fatalError, "Tag with id %d has no 3D size or there is no default 3D size defined", tagsId[i])); } - } else { + } + else { tagSize = it->second; } std::vector points3D(4); @@ -1035,7 +1037,8 @@ std::vector > vpDetectorAprilTag::getTagsPoints3D(const std points3D[1] = vpPoint(tagSize / 2, tagSize / 2, 0); points3D[2] = vpPoint(tagSize / 2, -tagSize / 2, 0); points3D[3] = vpPoint(-tagSize / 2, -tagSize / 2, 0); - } else { + } + else { points3D[0] = vpPoint(-tagSize / 2, -tagSize / 2, 0); points3D[1] = vpPoint(tagSize / 2, -tagSize / 2, 0); points3D[2] = vpPoint(tagSize / 2, tagSize / 2, 0); @@ -1144,9 +1147,9 @@ void vpDetectorAprilTag::setAprilTagQuadSigma(float quadSigma) { m_impl->setQuad #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! - Deprecated parameter from AprilTag 2 version. + \deprecated Deprecated parameter from AprilTag 2 version. */ -vp_deprecated void vpDetectorAprilTag::setAprilTagRefineDecode(bool refineDecode) +void vpDetectorAprilTag::setAprilTagRefineDecode(bool refineDecode) { m_impl->setRefineDecode(refineDecode); } @@ -1170,9 +1173,9 @@ void vpDetectorAprilTag::setAprilTagRefineEdges(bool refineEdges) { m_impl->setR #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! - Deprecated parameter from AprilTag 2 version. + \deprecated Deprecated parameter from AprilTag 2 version. */ -vp_deprecated void vpDetectorAprilTag::setAprilTagRefinePose(bool refinePose) { m_impl->setRefinePose(refinePose); } +void vpDetectorAprilTag::setAprilTagRefinePose(bool refinePose) { m_impl->setRefinePose(refinePose); } #endif void swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2) @@ -1195,5 +1198,5 @@ void vpDetectorAprilTag::setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame) #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has // no symbols -void dummy_vpDetectorAprilTag() {} +void dummy_vpDetectorAprilTag() { } #endif diff --git a/modules/gui/CMakeLists.txt b/modules/gui/CMakeLists.txt index 8972c4037b..406893195e 100644 --- a/modules/gui/CMakeLists.txt +++ b/modules/gui/CMakeLists.txt @@ -65,22 +65,8 @@ endif() if(USE_PCL) list(APPEND opt_incs ${PCL_INCLUDE_DIRS}) - - # list(APPEND opt_libs ${PCL_LIBRARIES}) - # Using PCL_LIBRARIES works to build visp library, examples, demos and test thanks to the components, - # but not tutorials when they are build outside ViSP as they are stand alone CMake projects that use - # ViSP as a 3rd party. - # To be clear PCL_LIBRARIES contains VTK 3rd party such as vtkalglib and not /usr/local/Cellar/vtk/6.3.0/lib/libvtkalglib-6.3.1.dylib - # full path as requested to use ViSP as 3rd party. This is the case for all VTK libraries that are PCL dependencies. - # The build of ViSP works with PCL_LIBRARIES since in that case thanks to vtkalglib properties, CMake - # is able to find the real name and location of the libraries. - # But when ViSP is used as a 3rd party where it should import PCL libraries, it doesn't work with - # PCL_LIBRARIES and especially with VTK_LIBRARIES. - # The solution here is to get the full location of VTK_LIBRARIES libraries thanks to the properties and link - # with these names. - # An other way could be to include PCLConfig.cmake, but in that case, visp-config and visp.pc - # will be not able to give the names of PCL libraries when used without CMake. - vp_find_pcl(PCL_LIBRARIES PCL_DEPS_INCLUDE_DIRS PCL_DEPS_LIBRARIES) + # To ensure to build with VTK and other PCL 3rd parties we are not using PCL_LIBRARIES but PCL_DEPS_INCLUDE_DIRS + # and PCL_DEPS_LIBRARIES instead list(APPEND opt_incs ${PCL_DEPS_INCLUDE_DIRS}) list(APPEND opt_libs ${PCL_DEPS_LIBRARIES}) endif() diff --git a/modules/gui/include/visp3/gui/vpDisplayGTK.h b/modules/gui/include/visp3/gui/vpDisplayGTK.h index cef6d6611a..c03b668dff 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGTK.h +++ b/modules/gui/include/visp3/gui/vpDisplayGTK.h @@ -184,7 +184,6 @@ class VISP_EXPORT vpDisplayGTK : public vpDisplay void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1) override; - void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green) override; void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; @@ -209,6 +208,8 @@ class VISP_EXPORT vpDisplayGTK : public vpDisplay bool fill = false, unsigned int thickness = 1) override; void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; + void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color = vpColor::green) override; + void flushDisplay() override; void flushDisplayROI(const vpImagePoint &iP, unsigned int width, unsigned int height) override; diff --git a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h index ddc2cdab4b..d7616526a6 100644 --- a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h +++ b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h @@ -225,8 +225,6 @@ class VISP_EXPORT vpDisplayOpenCV : public vpDisplay void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1) override; - void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green) override; - void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1) override; @@ -250,6 +248,8 @@ class VISP_EXPORT vpDisplayOpenCV : public vpDisplay bool fill = false, unsigned int thickness = 1) override; void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; + void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color = vpColor::green) override; + void flushDisplay() override; void flushDisplayROI(const vpImagePoint &iP, unsigned int width, unsigned int height) override; diff --git a/modules/gui/include/visp3/gui/vpDisplayWin32.h b/modules/gui/include/visp3/gui/vpDisplayWin32.h index 437a803b23..d78d0f2f1d 100644 --- a/modules/gui/include/visp3/gui/vpDisplayWin32.h +++ b/modules/gui/include/visp3/gui/vpDisplayWin32.h @@ -155,8 +155,6 @@ class VISP_EXPORT vpDisplayWin32 : public vpDisplay void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1) override; - void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green) override; - void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; @@ -175,6 +173,8 @@ class VISP_EXPORT vpDisplayWin32 : public vpDisplay bool fill = false, unsigned int thickness = 1) override; void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; + void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color = vpColor::green) override; + bool getClick(bool blocking = true) override; bool getClick(vpImagePoint &ip, bool blocking = true) override; bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true) override; diff --git a/modules/gui/include/visp3/gui/vpDisplayX.h b/modules/gui/include/visp3/gui/vpDisplayX.h index 3d2b2d3b55..947064f680 100644 --- a/modules/gui/include/visp3/gui/vpDisplayX.h +++ b/modules/gui/include/visp3/gui/vpDisplayX.h @@ -175,8 +175,6 @@ class VISP_EXPORT vpDisplayX : public vpDisplay void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1) override; - void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green) override; - void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1) override; @@ -200,6 +198,8 @@ class VISP_EXPORT vpDisplayX : public vpDisplay bool fill = false, unsigned int thickness = 1) override; void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1) override; + void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color = vpColor::green) override; + void flushDisplay() override; void flushDisplayROI(const vpImagePoint &iP, unsigned int width, unsigned int height) override; diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp index 7e4fa7da78..fc7ab4cccc 100644 --- a/modules/gui/src/display/vpDisplayGTK.cpp +++ b/modules/gui/src/display/vpDisplayGTK.cpp @@ -276,7 +276,7 @@ class vpDisplayGTK::Impl gdk_flush(); } - void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color, unsigned int scale) + void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color, unsigned int scale) { if (color.id < vpColor::id_unknown) gdk_gc_set_foreground(m_gc, m_col[color.id]); @@ -289,7 +289,7 @@ class vpDisplayGTK::Impl } if (m_font != nullptr) gdk_draw_string(m_background, m_font, m_gc, vpMath::round(ip.get_u() / scale), vpMath::round(ip.get_v() / scale), - (const gchar *)text); + (const gchar *)text.c_str()); else std::cout << "Cannot draw string: no font is selected" << std::endl; } @@ -818,14 +818,14 @@ void vpDisplayGTK::init(unsigned int win_width, unsigned int win_height, int win /*! Set the font used to display a text in overlay. The display is - performed using displayCharString(). + performed using displayText(). \param fontname : The expected font name. \note Under UNIX, to know all the available fonts, use the "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary. - \sa displayCharString() + \sa displayText() */ void vpDisplayGTK::setFont(const std::string &fontname) { m_impl->setFont(fontname); } @@ -1089,15 +1089,16 @@ void vpDisplayGTK::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2 \sa setFont() */ -void vpDisplayGTK::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) +void vpDisplayGTK::displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color) { if (m_displayHasBeenInitialized) { - m_impl->displayCharString(ip, text, color, m_scale); + m_impl->displayText(ip, text, color, m_scale); } else { throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } + /*! Display a circle. \param center : Circle center position. diff --git a/modules/gui/src/display/vpDisplayOpenCV.cpp b/modules/gui/src/display/vpDisplayOpenCV.cpp index 21b7ab1daa..f5b570e8b5 100644 --- a/modules/gui/src/display/vpDisplayOpenCV.cpp +++ b/modules/gui/src/display/vpDisplayOpenCV.cpp @@ -514,7 +514,7 @@ void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, const s \warning This method is not yet implemented. Set the font used to display a text in overlay. The display is - performed using displayCharString(). + performed using displayText(). \param font : The expected font name. The available fonts are given by the "xlsfonts" binary. To choose a font you can also use the @@ -523,7 +523,7 @@ void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, const s \note Under UNIX, to know all the available fonts, use the "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary. - \sa displayCharString() + \sa displayText() */ void vpDisplayOpenCV::setFont(const std::string & /* font */) { vpERROR_TRACE("Not yet implemented"); } @@ -1137,7 +1137,7 @@ void vpDisplayOpenCV::displayArrow(const vpImagePoint &ip1, const vpImagePoint & \sa setFont() */ -void vpDisplayOpenCV::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) +void vpDisplayOpenCV::displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) { diff --git a/modules/gui/src/display/vpDisplayX.cpp b/modules/gui/src/display/vpDisplayX.cpp index 17892fd412..b49df221ed 100644 --- a/modules/gui/src/display/vpDisplayX.cpp +++ b/modules/gui/src/display/vpDisplayX.cpp @@ -76,12 +76,11 @@ class vpDisplayX::Impl public: Impl() : display(nullptr), window(), Ximage(nullptr), lut(), context(), screen(0), event(), pixmap(), x_color(nullptr), - screen_depth(8), xcolor(), values(), ximage_data_init(false), RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), - BShift(0) - { - } + screen_depth(8), xcolor(), values(), ximage_data_init(false), RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), + BShift(0) + { } - ~Impl() {} + ~Impl() { } void clearDisplay(const vpColor &color, unsigned int width, unsigned int height) { @@ -123,7 +122,7 @@ class vpDisplayX::Impl } } - void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color, unsigned int scale) + void displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color, unsigned int scale) { if (color.id < vpColor::id_unknown) XSetForeground(display, context, x_color[color.id]); @@ -135,8 +134,8 @@ class vpDisplayX::Impl XAllocColor(display, lut, &xcolor); XSetForeground(display, context, xcolor.pixel); } - XDrawString(display, pixmap, context, (int)(ip.get_u() / scale), (int)(ip.get_v() / scale), text, - (int)strlen(text)); + XDrawString(display, pixmap, context, (int)(ip.get_u() / scale), (int)(ip.get_v() / scale), text.c_str(), + (int)text.size()); } void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill, @@ -159,7 +158,8 @@ class vpDisplayX::Impl XDrawArc(display, pixmap, context, vpMath::round((center.get_u() - radius) / scale), vpMath::round((center.get_v() - radius) / scale), radius * 2 / scale, radius * 2 / scale, 0, 23040); /* 23040 = 360*64 */ - } else { + } + else { XFillArc(display, pixmap, context, vpMath::round((center.get_u() - radius) / scale), vpMath::round((center.get_v() - radius) / scale), radius * 2 / scale, radius * 2 / scale, 0, 23040); /* 23040 = 360*64 */ @@ -207,9 +207,10 @@ class vpDisplayX::Impl dst_8[i] = nivGris; i++; } - } else { - // Correction de l'image de facon a liberer les niveaux de gris - // ROUGE, VERT, BLEU, JAUNE + } + else { + // Correction de l'image de facon a liberer les niveaux de gris + // ROUGE, VERT, BLEU, JAUNE unsigned char *dst_8 = (unsigned char *)Ximage->data; unsigned int k = 0; for (unsigned int i = 0; i < height; i++) { @@ -238,7 +239,8 @@ class vpDisplayX::Impl *(dst_16 + j) = (unsigned short)colortable[I[i][j]]; } } - } else { + } + else { for (unsigned int i = 0; i < height; i++) { unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; @@ -272,8 +274,9 @@ class vpDisplayX::Impl *(dst_32++) = val; // Green *(dst_32++) = val; // Blue } - } else { - // little endian + } + else { + // little endian while (bitmap < n) { unsigned char val = *(bitmap++); *(dst_32++) = val; // Blue @@ -282,7 +285,8 @@ class vpDisplayX::Impl *(dst_32++) = vpRGBa::alpha_default; } } - } else { + } + else { if (XImageByteOrder(display) == 1) { // big endian for (unsigned int i = 0; i < height; i++) { @@ -294,8 +298,9 @@ class vpDisplayX::Impl *(dst_32++) = val; // Blue } } - } else { - // little endian + } + else { + // little endian for (unsigned int i = 0; i < height; i++) { for (unsigned int j = 0; j < width; j++) { unsigned char val = I[i * scale][j * scale]; @@ -333,11 +338,12 @@ class vpDisplayX::Impl g = bitmap->G; b = bitmap->B; *(dst_16 + j) = - (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); bitmap++; } } - } else { + } + else { for (unsigned int i = 0; i < height; i++) { unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; @@ -347,7 +353,7 @@ class vpDisplayX::Impl g = val.G; b = val.B; *(dst_16 + j) = - (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); bitmap++; } } @@ -377,8 +383,9 @@ class vpDisplayX::Impl *(dst_32++) = bitmap->B; bitmap++; } - } else { - // little endian + } + else { + // little endian for (unsigned int i = 0; i < sizeI; i++) { *(dst_32++) = bitmap->B; *(dst_32++) = bitmap->G; @@ -387,7 +394,8 @@ class vpDisplayX::Impl bitmap++; } } - } else { + } + else { if (XImageByteOrder(display) == 1) { // big endian for (unsigned int i = 0; i < height; i++) { @@ -399,8 +407,9 @@ class vpDisplayX::Impl *(dst_32++) = val.B; } } - } else { - // little endian + } + else { + // little endian for (unsigned int i = 0; i < height; i++) { for (unsigned int j = 0; j < width; j++) { vpRGBa val = I[i * scale][j * scale]; @@ -474,9 +483,10 @@ class vpDisplayX::Impl XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); - } else { - // Correction de l'image de facon a liberer les niveaux de gris - // ROUGE, VERT, BLEU, JAUNE + } + else { + // Correction de l'image de facon a liberer les niveaux de gris + // ROUGE, VERT, BLEU, JAUNE int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); @@ -517,7 +527,8 @@ class vpDisplayX::Impl XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); - } else { + } + else { int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); @@ -567,8 +578,9 @@ class vpDisplayX::Impl dst_32 = dst_32 + 4 * width; i++; } - } else { - // little endian + } + else { + // little endian unsigned int i = 0; while (i < h) { unsigned int j = 0; @@ -588,7 +600,8 @@ class vpDisplayX::Impl XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); - } else { + } + else { int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); @@ -611,8 +624,9 @@ class vpDisplayX::Impl *(dst_32++) = val; } } - } else { - // little endian + } + else { + // little endian for (unsigned int i = i_min_; i < i_max_; i++) { unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(i * 4 * width + j_min_ * 4); for (unsigned int j = j_min_; j < j_max_; j++) { @@ -650,12 +664,13 @@ class vpDisplayX::Impl unsigned int g = val.G; unsigned int b = val.B; *(dst_16 + j) = - (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); } } XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); - } else { + } + else { unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line; int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); @@ -676,7 +691,7 @@ class vpDisplayX::Impl unsigned int g = val.G; unsigned int b = val.B; *(dst_16 + j) = - (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); } } XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, i_min, j_max_ - j_min_, i_max_ - i_min_); @@ -720,8 +735,9 @@ class vpDisplayX::Impl i++; } - } else { - // little endian + } + else { + // little endian while (i < h) { unsigned int j = 0; while (j < w) { @@ -740,7 +756,8 @@ class vpDisplayX::Impl XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); - } else { + } + else { int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); @@ -763,8 +780,9 @@ class vpDisplayX::Impl *(dst_32++) = val.B; } } - } else { - // little endian + } + else { + // little endian for (unsigned int i = i_min_; i < i_max_; i++) { unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(i * 4 * width + j_min_ * 4); for (unsigned int j = j_min_; j < j_max_; j++) { @@ -823,7 +841,8 @@ class vpDisplayX::Impl if (thickness == 1) { XDrawPoint(display, pixmap, context, vpMath::round(ip.get_u() / scale), vpMath::round(ip.get_v() / scale)); - } else { + } + else { XFillRectangle(display, pixmap, context, vpMath::round(ip.get_u() / scale), vpMath::round(ip.get_v() / scale), thickness, thickness); } @@ -846,7 +865,8 @@ class vpDisplayX::Impl if (fill == false) { XDrawRectangle(display, pixmap, context, vpMath::round(topLeft.get_u() / scale), vpMath::round(topLeft.get_v() / scale), w / scale, h / scale); - } else { + } + else { XFillRectangle(display, pixmap, context, vpMath::round(topLeft.get_u() / scale), vpMath::round(topLeft.get_v() / scale), w / scale, h / scale); } @@ -877,7 +897,8 @@ class vpDisplayX::Impl XCheckMaskEvent(display, ButtonReleaseMask, &event); XMaskEvent(display, ButtonPressMask, &event); ret = true; - } else { + } + else { ret = XCheckMaskEvent(display, ButtonPressMask, &event); } @@ -916,7 +937,8 @@ class vpDisplayX::Impl XCheckMaskEvent(display, ButtonReleaseMask, &event); XMaskEvent(display, ButtonReleaseMask, &event); ret = true; - } else { + } + else { ret = XCheckMaskEvent(display, ButtonReleaseMask, &event); } @@ -971,7 +993,8 @@ class vpDisplayX::Impl } } - } else { + } + else { if (XImageByteOrder(display) == 1) { // big endian for (unsigned int i = 0; i < width * height; i++) { @@ -983,8 +1006,9 @@ class vpDisplayX::Impl I.bitmap[i].G = src_32[i * 4 + 2]; I.bitmap[i].B = src_32[i * 4 + 3]; } - } else { - // little endian + } + else { + // little endian for (unsigned int i = 0; i < width * height; i++) { I.bitmap[i].B = src_32[i * 4]; I.bitmap[i].G = src_32[i * 4 + 1]; @@ -1007,7 +1031,8 @@ class vpDisplayX::Impl if (blocking) { XMaskEvent(display, KeyPressMask, &event); ret = true; - } else { + } + else { ret = XCheckMaskEvent(display, KeyPressMask, &event); } @@ -1028,7 +1053,8 @@ class vpDisplayX::Impl /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, &compose_status); key = buffer; ret = true; - } else { + } + else { ret = XCheckMaskEvent(display, KeyPressMask, &event); if (ret) { /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, &compose_status); @@ -1141,7 +1167,8 @@ class vpDisplayX::Impl // Positionnement de la fenetre dans l'ecran. if ((win_x < 0) || (win_y < 0)) { hints.flags = 0; - } else { + } + else { hints.flags = USPosition; hints.x = win_x; hints.y = win_y; @@ -1565,7 +1592,8 @@ class vpDisplayX::Impl Font stringfont; stringfont = XLoadFont(display, fontname.c_str()); //"-adobe-times-bold-r-normal--18*"); XSetFont(display, context, stringfont); - } catch (...) { + } + catch (...) { throw(vpDisplayException(vpDisplayException::notInitializedError, "Bad font")); } } @@ -1749,7 +1777,7 @@ int main() } \endcode */ -vpDisplayX::vpDisplayX() : vpDisplay(), m_impl(new Impl()) {} +vpDisplayX::vpDisplayX() : vpDisplay(), m_impl(new Impl()) { } /*! Destructor. @@ -1847,7 +1875,7 @@ void vpDisplayX::init(unsigned int win_width, unsigned int win_height, int win_x /*! Set the font used to display a text in overlay. The display is - performed using displayCharString(). + performed using displayText(). \param fontname : The expected font name. The available fonts are given by the "xlsfonts" binary. To choose a font you can also use the @@ -1856,7 +1884,7 @@ void vpDisplayX::init(unsigned int win_width, unsigned int win_height, int win_x \note Under UNIX, to know all the available fonts, use the "xlsfonts" binary in a terminal. You can also use the "xfontsel" binary. - \sa displayCharString() + \sa displayText() */ void vpDisplayX::setFont(const std::string &fontname) { @@ -1864,7 +1892,8 @@ void vpDisplayX::setFont(const std::string &fontname) if (!fontname.empty()) { m_impl->setFont(fontname); } - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1879,7 +1908,8 @@ void vpDisplayX::setTitle(const std::string &title) m_title = title; if (!title.empty()) m_impl->setTitle(title); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1897,7 +1927,8 @@ void vpDisplayX::setWindowPosition(int win_x, int win_y) { if (m_displayHasBeenInitialized) { m_impl->setWindowPosition(win_x, win_y); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1917,7 +1948,8 @@ void vpDisplayX::displayImage(const vpImage &I) { if (m_displayHasBeenInitialized) { m_impl->displayImage(I, m_scale, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1937,7 +1969,8 @@ void vpDisplayX::displayImage(const vpImage &I) { if (m_displayHasBeenInitialized) { m_impl->displayImage(I, m_scale, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1957,7 +1990,8 @@ void vpDisplayX::displayImage(const unsigned char *bitmap) { if (m_displayHasBeenInitialized) { m_impl->displayImage(bitmap, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1982,7 +2016,8 @@ void vpDisplayX::displayImageROI(const vpImage &I, const vpImageP { if (m_displayHasBeenInitialized) { m_impl->displayImageROI(I, iP, w, h, m_scale, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2006,7 +2041,8 @@ void vpDisplayX::displayImageROI(const vpImage &I, const vpImagePoint &i { if (m_displayHasBeenInitialized) { m_impl->displayImageROI(I, iP, w, h, m_scale, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2036,7 +2072,8 @@ void vpDisplayX::flushDisplay() { if (m_displayHasBeenInitialized) { m_impl->flushDisplay(); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2052,7 +2089,8 @@ void vpDisplayX::flushDisplayROI(const vpImagePoint &iP, unsigned int w, unsigne { if (m_displayHasBeenInitialized) { m_impl->flushDisplayROI(iP, w, h, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2065,7 +2103,8 @@ void vpDisplayX::clearDisplay(const vpColor &color) { if (m_displayHasBeenInitialized) { m_impl->clearDisplay(color, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2089,7 +2128,8 @@ void vpDisplayX::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, if ((std::fabs(a) <= std::numeric_limits::epsilon()) && (std::fabs(b) <= std::numeric_limits::epsilon())) { // DisplayCrossLarge(i1,j1,3,col) ; - } else { + } + else { a /= lg; b /= lg; @@ -2112,7 +2152,8 @@ void vpDisplayX::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, displayLine(ip1, ip2, color, thickness); } - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2128,11 +2169,12 @@ void vpDisplayX::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, \sa setFont() */ -void vpDisplayX::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) +void vpDisplayX::displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color) { if (m_displayHasBeenInitialized) { - m_impl->displayCharString(ip, text, color, m_scale); - } else { + m_impl->displayText(ip, text, color, m_scale); + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2153,7 +2195,8 @@ void vpDisplayX::displayCircle(const vpImagePoint ¢er, unsigned int radius, if (thickness == 1) thickness = 0; m_impl->displayCircle(center, radius, color, fill, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2185,7 +2228,8 @@ void vpDisplayX::displayCross(const vpImagePoint &ip, unsigned int cross_size, c ip2.set_j(j + cross_size / 2); displayLine(ip1, ip2, color, thickness); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2203,7 +2247,8 @@ void vpDisplayX::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2 thickness = 0; m_impl->displayDotLine(ip1, ip2, color, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2221,7 +2266,8 @@ void vpDisplayX::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c if (thickness == 1) thickness = 0; m_impl->displayLine(ip1, ip2, color, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2236,7 +2282,8 @@ void vpDisplayX::displayPoint(const vpImagePoint &ip, const vpColor &color, unsi { if (m_displayHasBeenInitialized) { m_impl->displayPoint(ip, color, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2262,7 +2309,8 @@ void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, unsigned int w, u thickness = 0; m_impl->displayRectangle(topLeft, w, h, color, fill, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2290,7 +2338,8 @@ void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, const vpImagePoin unsigned int h = static_cast(vpMath::round(bottomRight.get_v() - topLeft.get_v())); m_impl->displayRectangle(topLeft, w, h, color, fill, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2316,7 +2365,8 @@ void vpDisplayX::displayRectangle(const vpRect &rectangle, const vpColor &color, unsigned int w = static_cast(vpMath::round(rectangle.getWidth())); unsigned int h = static_cast(vpMath::round(rectangle.getHeight())); m_impl->displayRectangle(topLeft, w, h, color, fill, thickness, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2344,7 +2394,8 @@ bool vpDisplayX::getClick(bool blocking) vpImagePoint ip; vpMouseButton::vpMouseButtonType button; ret = m_impl->getClick(ip, button, blocking, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2372,7 +2423,8 @@ bool vpDisplayX::getClick(vpImagePoint &ip, bool blocking) if (m_displayHasBeenInitialized) { vpMouseButton::vpMouseButtonType button; ret = m_impl->getClick(ip, button, blocking, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2403,7 +2455,8 @@ bool vpDisplayX::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &bu if (m_displayHasBeenInitialized) { ret = m_impl->getClick(ip, button, blocking, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2438,7 +2491,8 @@ bool vpDisplayX::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType & if (m_displayHasBeenInitialized) { ret = m_impl->getClickUp(ip, button, blocking, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2455,7 +2509,8 @@ void vpDisplayX::getImage(vpImage &I) { if (m_displayHasBeenInitialized) { m_impl->getImage(I, m_width, m_height); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2517,7 +2572,8 @@ bool vpDisplayX::getKeyboardEvent(bool blocking) if (m_displayHasBeenInitialized) { ret = m_impl->getKeyboardEvent(blocking); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2551,7 +2607,8 @@ bool vpDisplayX::getKeyboardEvent(std::string &key, bool blocking) bool ret = false; if (m_displayHasBeenInitialized) { ret = m_impl->getKeyboardEvent(key, blocking); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2574,7 +2631,8 @@ bool vpDisplayX::getPointerMotionEvent(vpImagePoint &ip) bool ret = false; if (m_displayHasBeenInitialized) { ret = m_impl->getPointerMotionEvent(ip, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2595,7 +2653,8 @@ bool vpDisplayX::getPointerPosition(vpImagePoint &ip) bool ret = false; if (m_displayHasBeenInitialized) { ret = m_impl->getPointerPosition(ip, m_scale); - } else { + } + else { throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; @@ -2604,5 +2663,5 @@ bool vpDisplayX::getPointerPosition(vpImagePoint &ip) #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpDisplayX.cpp.o) has no // symbols -void dummy_vpDisplayX(){}; +void dummy_vpDisplayX() { }; #endif diff --git a/modules/gui/src/display/windows/vpDisplayWin32.cpp b/modules/gui/src/display/windows/vpDisplayWin32.cpp index dc0ffd0e4c..410db2674f 100644 --- a/modules/gui/src/display/windows/vpDisplayWin32.cpp +++ b/modules/gui/src/display/windows/vpDisplayWin32.cpp @@ -59,7 +59,7 @@ void vpCreateWindow(threadParam *param) /*! Constructor. */ -vpDisplayWin32::vpDisplayWin32(vpWin32Renderer *rend) : iStatus(false), window(rend) {} +vpDisplayWin32::vpDisplayWin32(vpWin32Renderer *rend) : iStatus(false), window(rend) { } /*! Destructor. @@ -301,7 +301,8 @@ bool vpDisplayWin32::getClick(bool blocking) WaitForSingleObject(window.semaClickUp, 0); // to erase previous events WaitForSingleObject(window.semaClick, INFINITE); ret = true; - } else { + } + else { ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0)); } @@ -339,7 +340,8 @@ bool vpDisplayWin32::getClick(vpImagePoint &ip, bool blocking) WaitForSingleObject(window.semaClickUp, 0); // to erase previous events WaitForSingleObject(window.semaClick, INFINITE); ret = true; - } else { + } + else { ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0)); } @@ -383,7 +385,8 @@ bool vpDisplayWin32::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType WaitForSingleObject(window.semaClickUp, 0); // to erase previous events WaitForSingleObject(window.semaClick, INFINITE); ret = true; - } else + } + else ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0)); u = window.clickX; @@ -432,7 +435,8 @@ bool vpDisplayWin32::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonTy WaitForSingleObject(window.semaClick, 0); // to erase previous events WaitForSingleObject(window.semaClickUp, INFINITE); ret = true; - } else + } + else ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClickUp, 0)); u = window.clickXUp; @@ -471,7 +475,8 @@ bool vpDisplayWin32::getKeyboardEvent(bool blocking) WaitForSingleObject(window.semaKey, 0); // key up WaitForSingleObject(window.semaKey, INFINITE); ret = true; - } else + } + else ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0)); return ret; @@ -507,7 +512,8 @@ bool vpDisplayWin32::getKeyboardEvent(std::string &key, bool blocking) WaitForSingleObject(window.semaKey, 0); // key up WaitForSingleObject(window.semaKey, INFINITE); ret = true; - } else { + } + else { ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0)); } // printf("key: %ud\n", window.key); @@ -664,7 +670,8 @@ void vpDisplayWin32::displayPoint(const vpImagePoint &ip, const vpColor &color, waitForInit(); if (thickness == 1) { window.renderer->setPixel(ip, color); - } else { + } + else { window.renderer->drawRect(ip, thickness * m_scale, thickness * m_scale, color, true, 1); } } @@ -788,11 +795,11 @@ void vpDisplayWin32::displayCircle(const vpImagePoint ¢er, unsigned int radi \param text : The string to display \param color : The text's color */ -void vpDisplayWin32::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) +void vpDisplayWin32::displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color) { // wait if the window is not initialized waitForInit(); - window.renderer->drawText(ip, text, color); + window.renderer->drawText(ip, text.c_str(), color); } /*! @@ -901,5 +908,5 @@ unsigned int vpDisplayWin32::getScreenHeight() #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpDisplayWin32.cpp.o) has no // symbols -void dummy_vpDisplayWin32(){}; +void dummy_vpDisplayWin32() { }; #endif diff --git a/modules/gui/test/display/testDisplays.cpp b/modules/gui/test/display/testDisplays.cpp index fe5c88ec35..f87ccdca0d 100644 --- a/modules/gui/test/display/testDisplays.cpp +++ b/modules/gui/test/display/testDisplays.cpp @@ -80,7 +80,7 @@ Test video devices or display.\n\ SYNOPSIS\n\ %s [-l] [-c] [-d] [-h]\n\ ", - name); +name); fprintf(stdout, "\n\ OPTIONS: Default\n\ @@ -247,18 +247,34 @@ template static void draw(vpImage &I) iP1.set_j(400); vpDisplay::displayRectangle(I, iP1, 45, w, h, vpColor::green, 3); - std::vector polygon; - polygon.push_back(vpImagePoint(250, 500)); - polygon.push_back(vpImagePoint(350, 600)); - polygon.push_back(vpImagePoint(450, 500)); - polygon.push_back(vpImagePoint(350, 400)); + std::vector vip; + vip.push_back(vpImagePoint(250, 500)); + vip.push_back(vpImagePoint(350, 600)); + vip.push_back(vpImagePoint(450, 500)); + vip.push_back(vpImagePoint(350, 400)); + vpDisplay::displayPolygon(I, vip, vpColor::green, 3); + + vip.clear(); + vip.push_back(vpImagePoint(300, 500)); + vip.push_back(vpImagePoint(350, 550)); + vip.push_back(vpImagePoint(400, 500)); + vip.push_back(vpImagePoint(350, 450)); + vpDisplay::displayPolygon(I, vip, vpColor::cyan, 3, false); + + vip.clear(); + vip.push_back(vpImagePoint(250, 300)); + vip.push_back(vpImagePoint(350, 400)); + vip.push_back(vpImagePoint(450, 300)); + vip.push_back(vpImagePoint(350, 200)); + vpPolygon polygon(vip); vpDisplay::displayPolygon(I, polygon, vpColor::green, 3); - polygon.clear(); - polygon.push_back(vpImagePoint(300, 500)); - polygon.push_back(vpImagePoint(350, 550)); - polygon.push_back(vpImagePoint(400, 500)); - polygon.push_back(vpImagePoint(350, 450)); + vip.clear(); + vip.push_back(vpImagePoint(300, 300)); + vip.push_back(vpImagePoint(350, 350)); + vip.push_back(vpImagePoint(400, 300)); + vip.push_back(vpImagePoint(350, 250)); + polygon.buildFrom(vip); vpDisplay::displayPolygon(I, polygon, vpColor::cyan, 3, false); } @@ -408,7 +424,8 @@ int main(int argc, const char **argv) runTest(opt_display, opt_click_allowed); return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getMessage() << std::endl; return EXIT_FAILURE; } diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index f1be206cd4..ea4839c104 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -28,6 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. */ +#include + #include #include #include @@ -476,9 +478,9 @@ vpCircleHoughTransform::computeCenterCandidates() for (int y = 0; y < nbRowsAccum; y++) { int left = -1; for (int x = 0; x < nbColsAccum; x++) { - if (centersAccum[y][x] >= m_algoParams.m_centerThresh - && centersAccum[y][x] == centerCandidatesMaxima[y][x] - && centersAccum[y][x] > centersAccum[y][x + 1] + if ((centersAccum[y][x] >= m_algoParams.m_centerThresh) + && (std::fabs(centersAccum[y][x] - centerCandidatesMaxima[y][x]) < std::numeric_limits::epsilon()) + && (centersAccum[y][x] > centersAccum[y][x + 1]) ) { if (left < 0) left = x; diff --git a/modules/robot/CMakeLists.txt b/modules/robot/CMakeLists.txt index c205a763b2..0287a79924 100644 --- a/modules/robot/CMakeLists.txt +++ b/modules/robot/CMakeLists.txt @@ -185,6 +185,12 @@ if(WITH_TAKKTILE2) list(APPEND opt_libs_private ${TAKKTILE2_LIBRARIES}) endif() +if(WITH_POLOLU) + # Rapa Pololu Maestro 3rdparty is private + include_directories(${POLOLU_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${POLOLU_LIBRARIES}) +endif() + vp_add_module(robot visp_core OPTIONAL visp_io visp_gui visp_sensor PRIVATE_OPTIONAL ${opt_libs_private}) vp_glob_module_sources() @@ -247,8 +253,11 @@ vp_glob_module_copy_data("src/wireframe-simulator/scene/*.sld" data/wireframe-si vp_module_include_directories(${opt_incs}) vp_create_module(${opt_libs}) vp_add_tests( - DEPENDS_ON visp_sensor visp_vision visp_blob visp_gui - CTEST_EXCLUDE_PATH bebop2 qbdevice servo-afma4 servo-afma6 servo-franka servo-universal-robots servo-viper virtuose servo-pixhawk) + DEPENDS_ON + visp_sensor visp_vision visp_blob visp_gui + CTEST_EXCLUDE_PATH + bebop2 qbdevice servo-afma4 servo-afma6 servo-franka servo-pixhawk servo-pololu servo-universal-robots + servo-viper virtuose) if(USE_VIRTUOSE) # Add specific build flag to turn off warnings coming from VirtuoseAPI 3rd party diff --git a/modules/robot/include/visp3/robot/vpPololu.h b/modules/robot/include/visp3/robot/vpPololu.h new file mode 100644 index 0000000000..372fa46cf1 --- /dev/null +++ b/modules/robot/include/visp3/robot/vpPololu.h @@ -0,0 +1,333 @@ +/* + * 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: + * Common features for Pololu Maestro Servo Motor. + */ + +#ifndef _vpPololu_h_ +#define _vpPololu_h_ + +#include + +#ifdef VISP_HAVE_POLOLU + +#include +#include +#include + +class RPMSerialInterface; + +/*! + * \class vpPololu + * \ingroup group_robot_real_arm + * + * \brief Interface for the Pololu Maestro USB Servo Controllers. + * + * See https://www.pololu.com/category/102/maestro-usb-servo-controllers for more details. + * + * This class give a position and velocity control for the servo motors plugged into the board on a given channel. + * If you want to control two or more servo motors, you need to instanciate a new object for each additional + * servo motor. An example is given in the vpRobotPololuPtu class that allows to control a pan-tilt unit with + * two servo motors. + * + * It implements a velocity controller that runs in a separate thread. + * + * - A servomotor can be position-controlled by giving it position commands in pwm units using setPwmPosition() + * or in angles expressed in radians using setAngularPosition(). + * - It can be also velocity-controller by giving it velocity commands in pwm units using setPwmVelocity() or in ras/s + * using setAngularVelocity(). + * + * The conversion between pwm units and radians positions is done using radToPwm() or pwmToRad(). + * For velocity control conversion from pwm units and rad/s is done using speedToRadS() and radSToSpeed(). + * + * Each servo has a pwm position range that could be retrieved using calibrate() and set using set using setPwmRange(). + * + * It is the user responsability to set the corresponding angular range using setAngularRange(). + */ +class VISP_EXPORT vpPololu +{ +public: + /*! + * Default constructor. + * + * \param[in] verbose : When true enable verbose mode. + * + * You need to call connect() to setup the serial link with the Pololu board. + */ + vpPololu(bool verbose = false); + + /*! + * Constructor that enables the serial link with the Pololu board calling internally connect(). + * The velocity controller thread is created in the constructor and will run independently of the rest of the class. + * You can set velocity commands using setPwmVelocity() or setAngularVelocity(). + * + * \param[in] device : Serial device name to dial with Pololu board. + * \param[in] baudrate : Baudrate used to dial with Pololu board. Note that this parameter is only used on Windows. + * \param[in] channel : Channel to which the servo is connected to the Pololu board. + * \param[in] verbose : When true enable verbose mode. + */ + vpPololu(const std::string &device, int baudrate = 38400, int channel = 0, bool verbose = false); + + /*! + * Destructor. + */ + virtual ~vpPololu(); + + /*! + * Move servo motor to minimal pwm position and then to maximal pwm position + * to retrieve min and max pwm values. + * + * \param[out] pwm_min : Min position (pwm). + * \param[out] pwm_max : Max position (pwm). + */ + void calibrate(unsigned short &pwm_min, unsigned short &pwm_max); + + /*! + * Open a connection with the Pololu board. + * + * \param[in] device : Serial device name to dial with Pololu board. + * \param[in] baudrate : Baudrate used to dial with Pololu board. Note that this parameter is only used on Windows. + * \param[in] channel : Channel to which the servo is connected to the Pololu board. + */ + void connect(const std::string &device, int baudrate, int channel); + + /*! + * Check if the serial connection is still up. + * + * \return true is the connection is enabled, false if the board is not connected. + */ + bool connected() const; + + /*! + * Return angular position in rad. + * + * \return Current position in rad. + */ + float getAngularPosition() const; + + /*! + * Return PWM position. + * + * \return Current PWM position. + */ + unsigned short getPwmPosition() const; + + /*! + * Get min, max and range for angle cmd. + * + * \param[out] minAngle : Min range value for angle control. + * + * \param[out] maxAngle : Max range value for angle control. + * + * \sa setAngularRange() + */ + void getRangeAngles(float &minAngle, float &maxAngle) const; + + /*! + * Get min, max range for PWM cmd. + * + * \param[out] min : Min value for PWM control. + * + * \param[out] max: Max value for PWM control. + * + * \sa setPwmRange() + */ + void getRangePwm(unsigned short &min, unsigned short &max); + + /*! + * Set the position to reach in angle. + * + * \param[in] pos_rad : Position to reach in radians. + * + * \param[in] vel_rad_s : Velocity to use for the positioning in rad/s. Default is '0' and will use + * maximum speed. + */ + void setAngularPosition(float pos_rad, float vel_rad_s = 0.f); + + /*! + * Set min and max axis angles range in rad. + * + * \param[in] min_angle : Min value for angle (rad). + * + * \param[in] max_angle : Max value for angle (rad). + * + * \sa getRangeAngles() + */ + inline void setAngularRange(float min_angle, float max_angle) + { + m_min_angle = min_angle; + m_max_angle = max_angle; + m_range_angle = m_max_angle - m_min_angle; + } + + /*! + * Set the angular velocity of the motor movements in rad/s. + * + * \param[in] vel_rad_s : Velocity to apply for movement in rad/s. + */ + void setAngularVelocity(float vel_rad_s); + + /*! + * Set the position to reach in PWM. + * + * \param[in] pos_pwm : Position in PWM to reach. + * + * \param[in] speed_pwm : Speed to use for movement in units of (0.25 μs)/(10 ms). Default is 0, maximum speed. + * + * \exception When PWM out of range. + */ + void setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm = 0); + + /*! + * Set min, max PWM cmd. + * + * \param[in] min_pwm : Min value for PWM control. + * + * \param[in] max_pwm : Max value for PWM control. + * + * \sa getRangePwm() + */ + inline void setPwmRange(unsigned short min_pwm, unsigned short max_pwm) + { + m_min_pwm = min_pwm; + m_max_pwm = max_pwm; + m_range_pwm = m_max_pwm - m_min_pwm; + } + + /*! + * Set the pwm velocity of the motor movements. The motor will move to the edge of the + * range at the given speed. + * + * \param[in] pwm_vel : PWM velocity to use for movement in units of (0.25 μs)/(10 ms). When set to 0, will use the + * maximum speed. + */ + void setPwmVelocity(short pwm_vel); + + /*! + * Enable/disable verbose mode. + * + * \param[in] verbose : Set to true to enable verbose mode, false otherwise. + */ + void setVerbose(bool verbose) + { + m_verbose = verbose; + } + + /*! + * Stop the velocity command thread. + */ + void stopVelocityCmd(); + + /*! + * @name Public Member Functions for Conversion + */ + //@{ + /*! + * Convert a PWM value to an angle in radians. + * + * \param[in] pwm : PWM value. + * + * \return Corresponding angle value in radian for the PWM. + * + * \sa radToPwm() + */ + float pwmToRad(unsigned short pwm) const; + + /*! + * Convert angles in radians to PWM for servo commands. + * + * \param angle : Angle in radian to convert. + * + * \return Corresponding PWM value for the angle. + * + * \sa pwmToRad() + */ + unsigned short radToPwm(float angle) const; + + /*! + * Convert deg/s speed into Pololu's speed. + * + * \param speed_rad_s : Speed converted to rad/s. + * + * \return Signed speed in units of (0.25 μs)/(10 ms). + * + * \sa speedToRadS() + */ + short radSToSpeed(float speed_rad_s) const; + + /*! + * Convert Pololu's pwm velocity to rad/s velocity. + * + * \param[in] speed : Signed speed in units of (0.25 μs)/(10 ms). + * + * \return Speed converted to rad/s. + * + * \sa radSToSpeed() + */ + float speedToRadS(short speed) const; + //@} + +private: + static RPMSerialInterface *m_interface; // Only one interface should be used even when controlling multiple servos + static int m_nb_servo; // Object counter to handel serial interface destruction + + int m_channel; + bool m_apply_velocity_cmd; + bool m_stop_velocity_cmd_thread; + + unsigned short m_vel_speed; //!< PWM speed to in velocity control + unsigned short m_vel_target_position; //!< Min or max PWM target position to reach in velocity control + + unsigned short m_vel_speed_prev; //!< Previous PWM speed to in velocity control + unsigned short m_vel_target_position_prev; //!< Previous Min or max PWM target position to reach in velocity control + + std::mutex m_mutex_velocity_cmd; + + // ranges + unsigned short m_min_pwm = 4095; + unsigned short m_max_pwm = 7905; + unsigned short m_range_pwm = m_max_pwm - m_min_pwm; + float m_min_angle = -40; + float m_max_angle = 40; + float m_range_angle = abs(m_min_angle) + abs(m_max_angle); + + bool m_verbose; + + /*! + * Thread use for Velocity control. This thread is launch in the constructor of the object and, unless crashes, will + * run until the process is ended. If the m_apply_velocity_cmd is set to TRUE, by invoking the setPwmVelocity method, the + * motor will go to the edge of the motor range using the speed set in setPwmVelocity. The velocity command can be + * stopped invoking the stopVelocityCmd() method. + */ + void VelocityCmdThread(); +}; + +#endif +#endif diff --git a/modules/robot/include/visp3/robot/vpRobot.h b/modules/robot/include/visp3/robot/vpRobot.h index 20dafb87c3..afe660e3ae 100644 --- a/modules/robot/include/visp3/robot/vpRobot.h +++ b/modules/robot/include/visp3/robot/vpRobot.h @@ -59,7 +59,8 @@ class VISP_EXPORT vpRobot /*! Robot control states. */ - typedef enum { + typedef enum + { STATE_STOP, //!< Stops robot motion especially in velocity and acceleration control. STATE_VELOCITY_CONTROL, //!< Initialize the velocity controller. STATE_POSITION_CONTROL, //!< Initialize the position controller. @@ -70,7 +71,8 @@ class VISP_EXPORT vpRobot /*! Robot control frames. */ - typedef enum { + typedef enum + { REFERENCE_FRAME, /*!< Corresponds to a fixed reference frame attached to the robot structure. */ ARTICULAR_FRAME, /*!< Corresponds to the joint state. This value is deprecated. @@ -134,6 +136,15 @@ class VISP_EXPORT vpRobot double getMaxTranslationVelocity(void) const; double getMaxRotationVelocity(void) const; + + /*! + * Return robot degrees of freedom number. + */ + int getNDof() const + { + return nDof; + } + //! Get the robot position (frame has to be specified). virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) = 0; diff --git a/modules/robot/include/visp3/robot/vpRobotException.h b/modules/robot/include/visp3/robot/vpRobotException.h index 28f1d2bc86..acf7b01cee 100644 --- a/modules/robot/include/visp3/robot/vpRobotException.h +++ b/modules/robot/include/visp3/robot/vpRobotException.h @@ -58,7 +58,7 @@ class VISP_EXPORT vpRobotException : public vpException enum errorRobotCodeEnum { -//! Error from constructor + //! Error from constructor constructionError, //! Not unique robot object construction diff --git a/modules/robot/include/visp3/robot/vpRobotMavsdk.h b/modules/robot/include/visp3/robot/vpRobotMavsdk.h index 217ce46729..5b6bba8156 100644 --- a/modules/robot/include/visp3/robot/vpRobotMavsdk.h +++ b/modules/robot/include/visp3/robot/vpRobotMavsdk.h @@ -38,7 +38,9 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher. +// Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include #include diff --git a/modules/robot/include/visp3/robot/vpRobotPololuPtu.h b/modules/robot/include/visp3/robot/vpRobotPololuPtu.h new file mode 100644 index 0000000000..d794cc4e5f --- /dev/null +++ b/modules/robot/include/visp3/robot/vpRobotPololuPtu.h @@ -0,0 +1,274 @@ +/* + * 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: + * Common features for Pololu Maestro PanTiltUnit. + */ + +#ifndef _vpRobotPololuPtu_h_ +#define _vpRobotPololuPtu_h_ + +#include + +#ifdef VISP_HAVE_POLOLU + +#include +#include + +using namespace std; + +/*! + * \class vpRobotPololuPtu + * \ingroup group_robot_real_arm + * + * \brief Interface for the Pololu Maestro pan-tilt unit using two servo motors. + * + * See https://www.pololu.com/category/102/maestro-usb-servo-controllers for more details. + * + * This class handle the vpPololu class in a higher level and allows to control + * the pan-tilt unit using position or velocity commands. + * + * The corresponding Denavit-Hartenberg representations of the PTU is the following: + * + * | Joint | \f$a_i\f$ | \f$d_i\f$ | \f$\alpha_i\f$ | \f$\theta_i\f$ | + * | :---: | :-------: | :-------: | -------------: | ----------------: | + * | 1 | 0 | 0 | \f$ \pi/2\f$ | \f$q_1\f$ | + * | 2 | 0 | 0 | \f$-\pi/2\f$ | \f$q_2 - \pi/2\f$ | + */ +class VISP_EXPORT vpRobotPololuPtu : public vpRobot +{ +public: + /*! + * Default constructor. + * + * \param[in] device : Name of the serial interface used for communication. + * \param[in] baudrate : Baudrate used for the serial communication. Note that this parameter is only used on Windows. + * \param[in] verbose : When true, enable verbose mode. + */ + vpRobotPololuPtu(const std::string &device = "/dev/ttyACM0", int baudrate = 9600, bool verbose = false); + + /*! + * Destructor that stops the movements. + */ + ~vpRobotPololuPtu() override; + + /*! + * Get the robot jacobian expressed in the end-effector frame. + * + * \warning End-effector frame is not the embedded camera frame. It corresponds to the frame + * associated to the tilt axis (see also get_cMe). + * + * \param[out] eJe : Jacobian between end effector frame and end effector frame (on + * tilt axis). + */ + void get_eJe(vpMatrix &eJe) override; + + /*! + * Get the robot jacobian expressed in the end-effector frame. + * + * \warning End-effector frame is not the embedded camera frame. It corresponds to the frame + * associated to the tilt axis (see also get_cMe). + * + * \param[in] q : Joint positions to consider [rad]. + * + * \param[out] eJe : Jacobian between end effector frame and end effector frame (on + * tilt axis). + */ + void get_eJe(const vpColVector &q, vpMatrix &eJe) const; + + /*! + * Get the robot jacobian expressed in the robot reference frame. + * + * \param[out] fJe : Jacobian between reference frame (or fix frame) and end + * effector frame (on tilt axis). + */ + void get_fJe(vpMatrix &fJe) override; + + /*! + * Get the robot jacobian expressed in the robot reference frame. + * + * \param[in] q : Joint positions to consider [rad]. + * + * \param[out] fJe : Jacobian between reference frame (or fix frame) and end + * effector frame (on tilt axis). + */ + void get_fJe(const vpColVector &q, vpMatrix &fJe) const; + + /*! + * Return the minimul angular velocity in rad/s that could be applied to move the motors. + * It corresponds to 1 pwm converted in rad/s. + */ + float getAngularVelocityResolution() const; + + /*! + * Return the position of each joint. + * + * \param[in] frame : Control frame. This PTU can only be controlled in + * joint state. + * + * \param[out] q : The position of the joints in radians. + * + * \exception vpRobotException::wrongStateError : If a not supported frame type + * is given. + */ + void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) override; + + /*! + * Get the percentage of the maximum velocity applied to move the PTU in position. + * + * \return Positioning velocity percentage in [0, 100.0]. The + * maximum positioning velocity is given vpRobot::getMaxRotationVelocity(). + * + * \sa setPositioningVelocityPercentage() + */ + float getPositioningVelocityPercentage() const + { + return m_positioning_velocity_percentage; + } + + /*! + * Move the robot to a given joint position. + * + * \warning This method is blocking. That mean that it waits the end of the + * positioning. + * + * \param[in] frame : Control frame. This PTU can only be controlled in + * joint state. + * + * \param[in] q : The joint position to set for each axis in radians. + * + * \exception vpRobotException::wrongStateError : If a not supported frame + * type is given. + */ + void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) override; + + /*! + * Set the percentage of the maximum velocity applied to move the PTU in position. + * + * \param[in] positioning_velocity_percentage : Percentage between [0,100] of the maximum velocity. The + * maximum positioning velocity is given vpRobot::getMaxRotationVelocity(). + * + * \sa getPositioningVelocityPercentage() + */ + void setPositioningVelocityPercentage(float positioning_velocity_percentage) + { + m_positioning_velocity_percentage = positioning_velocity_percentage; + } + + /*! + * Enable/disable verbose mode. + * + * \param[in] verbose : Set to true to enable verbose mode, false otherwise. + */ + void setVerbose(bool verbose) + { + m_verbose = verbose; + } + + /*! + * Send a velocity on each axis. + * + * \param[in] frame : Control frame. This Biclops head can only be controlled in + * joint state. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference + * frame (vpRobot::REFERENCE_FRAME), end-effector frame (vpRobot::END_EFFECTOR_FRAME) + * and the mixt frame (vpRobot::MIXT_FRAME) are not implemented. + * + * \param[in] q_dot : The desired joint velocities for each axis in rad/s. \f$ \dot + * {r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the + * camera and \f$ \dot{q}_2\f$ the tilt of the camera. + * + * \exception vpRobotException::wrongStateError : If a the robot is not + * configured to handle a velocity. The robot can handle a velocity only if the + * velocity control mode is set. For that, call setRobotState( + * vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). + * + * \exception vpRobotException::wrongStateError : If a not supported frame type + * (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME + * or vpRobot::MIXT_FRAME) is given. + * + * \warning Velocities could be saturated if one of them exceed the maximal + * authorized speed (see vpRobot::maxRotationVelocity). + */ + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) override; + + /*! + * Stop the velocity command. + */ + void stopVelocity(); + + /*! + * Change the state of the robot either to stop them, or to set position or + * speed control. + */ + vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) override; + +private: + /*! + * Initialize the robot. + * + * \exception vpRobotException::constructionError If the config file cannot be + * opened. + */ + void init() override { }; + + /*! + * Get the robot displacement since the last call of this method. + * + * \warning The first call of this method gives not a good value for the + * displacement. + * + * \param[in] frame The frame in which the measured displacement is expressed. + * + * \param[out] d The displacement: + * + * - In joint state, the dimension of q is 2 (the number of axis of the robot) + * with respectively d[0] (pan displacement), d[1] (tilt displacement). + * + * - In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz). + * Translations are expressed in meters, rotations in radians with the theta U + * representation. + * + * \exception vpRobotException::wrongStateError If a not supported frame type + * is given. + */ + void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d) override + { + (void)frame; + (void)d; + }; + + vpPololu m_pan; + vpPololu m_tilt; + float m_positioning_velocity_percentage; + + bool m_verbose; +}; + +#endif +#endif diff --git a/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp b/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp index a3adb092f4..c806b68a9f 100644 --- a/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp +++ b/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp @@ -47,10 +47,10 @@ class vpQbDevice::Impl { public: - Impl() : Impl(std::make_shared()) {} + Impl() : Impl(std::make_shared()) { } Impl(std::shared_ptr device_api) : m_serial_protectors(), m_connected_devices(), m_position_limits(2), m_device_api(device_api), - m_file_descriptors(), m_max_repeats(1), m_current_max(750.) + m_file_descriptors(), m_max_repeats(1), m_current_max(750.) { // Default values updated after a call to init() m_position_limits[0] = 0; @@ -62,7 +62,8 @@ class vpQbDevice::Impl for (auto it = m_file_descriptors.begin(); it != m_file_descriptors.end();) { if (close(it->first)) { it = m_file_descriptors.erase(it); - } else { + } + else { ++it; } } @@ -112,7 +113,7 @@ class vpQbDevice::Impl public: std::map > - m_serial_protectors; // only callbacks must lock the serial resources + m_serial_protectors; // only callbacks must lock the serial resources std::map m_connected_devices; protected: @@ -228,8 +229,8 @@ int vpQbDevice::Impl::getMeasurements(const int &id, const int &max_repeats, std int vpQbDevice::Impl::getParameters(const int &id, std::vector &limits, std::vector &resolutions) { - std::vector input_mode = {-1}; - std::vector control_mode = {-1}; + std::vector input_mode = { -1 }; + std::vector control_mode = { -1 }; m_device_api->getParameters(&m_file_descriptors.at(m_connected_devices.at(id)), id, input_mode, control_mode, resolutions, limits); if (!input_mode.front() && !control_mode.front()) { // both input and control modes equals 0 are required, i.e. @@ -276,7 +277,8 @@ int vpQbDevice::Impl::getSerialPortsAndDevices(const int &max_repeats) } // 'serial_protectors_' is not cleared because of the previously acquired lock, do not do it! -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) + // Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) m_serial_protectors.insert(std::make_pair(serial_ports.at(i), std::make_unique())); // never override #else m_serial_protectors.insert( @@ -309,7 +311,7 @@ bool vpQbDevice::Impl::init(const int &id) { std::vector encoder_resolutions; std::vector > - serial_locks; // need to lock on all the serial resources to scan for new ports/devices + serial_locks; // need to lock on all the serial resources to scan for new ports/devices for (auto const &mutex : m_serial_protectors) { serial_locks.push_back(std::unique_lock(*mutex.second)); } @@ -326,7 +328,7 @@ bool vpQbDevice::Impl::init(const int &id) if (getParameters(id, position_limits, encoder_resolutions)) { std::cout << "fails while initializing device [" << id - << "] because it requires 'USB' input mode and 'Position' control mode." << std::endl; + << "] because it requires 'USB' input mode and 'Position' control mode." << std::endl; return false; } @@ -363,13 +365,13 @@ bool vpQbDevice::Impl::init(const int &id) failures = activate(id, m_max_repeats); if (!isReliable(failures, m_max_repeats)) { std::cout << "has not initialized device [" << id - << "] because it cannot activate its motors (please, check the motor positions)." << std::endl; + << "] because it cannot activate its motors (please, check the motor positions)." << std::endl; return false; } std::string serial_port = m_connected_devices.at(id); std::cout << "Device [" + std::to_string(id) + "] connected on port [" << serial_port << "] initialization succeeds." - << std::endl; + << std::endl; return true; } @@ -410,19 +412,19 @@ int vpQbDevice::Impl::open(const std::string &serial_port) #if (defined(__APPLE__) && defined(__MACH__)) if (!std::regex_match(serial_port, std::regex("/dev/tty.usbserial-[[:digit:]]+"))) { std::cout << "vpQbDevice fails while opening [" << serial_port - << "] because it does not match the expected pattern [/dev/tty.usbserial-*]." << std::endl; + << "] because it does not match the expected pattern [/dev/tty.usbserial-*]." << std::endl; return -1; } #elif defined(__unix__) || defined(__unix) if (!std::regex_match(serial_port, std::regex("/dev/ttyUSB[[:digit:]]+"))) { std::cout << "vpQbDevice fails while opening [" << serial_port - << "] because it does not match the expected pattern [/dev/ttyUSB*]." << std::endl; + << "] because it does not match the expected pattern [/dev/ttyUSB*]." << std::endl; return -1; } #elif defined(_WIN32) if (!std::regex_match(serial_port, std::regex("COM[[:digit:]]+"))) { std::cout << "vpQbDevice fails while opening [" << serial_port - << "] because it does not match the expected pattern [COM*]." << std::endl; + << "] because it does not match the expected pattern [COM*]." << std::endl; return -1; } #endif @@ -435,8 +437,8 @@ int vpQbDevice::Impl::open(const std::string &serial_port) m_device_api->open(&m_file_descriptors[serial_port], serial_port); // also create a pair in the map if (m_file_descriptors.at(serial_port).file_handle == INVALID_HANDLE_VALUE) { std::cout << "vpQbDevice fails while opening [" << serial_port << "] and sets errno [" << strerror(errno) << "]." - << std::endl; - // remove file descriptor entry + << std::endl; +// remove file descriptor entry m_file_descriptors.erase(serial_port); return -1; } diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index c94b8322ba..1aafc581e8 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -35,7 +35,8 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include #include @@ -193,7 +194,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl calibration_promise.set_value(); break; } - }; + }; } void calibrate_gyro(mavsdk::Calibration &calibration) diff --git a/modules/robot/src/real-robot/pololu-maestro/vpPololu.cpp b/modules/robot/src/real-robot/pololu-maestro/vpPololu.cpp new file mode 100644 index 0000000000..ad20ba4d31 --- /dev/null +++ b/modules/robot/src/real-robot/pololu-maestro/vpPololu.cpp @@ -0,0 +1,338 @@ +/* + * 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: + * Common features for Pololu Maestro Servo Motor. + */ + +#include + + +#ifdef VISP_HAVE_POLOLU + +#include +#include + +#include + +#include +#include +#include + +std::chrono::milliseconds millis(1); + +RPMSerialInterface *vpPololu::m_interface = nullptr; +int vpPololu::m_nb_servo = 0; + +vpPololu::vpPololu(bool verbose) + : m_channel(0), m_apply_velocity_cmd(false), m_stop_velocity_cmd_thread(false), + m_vel_speed(1), m_vel_target_position(0), m_vel_speed_prev(1), m_vel_target_position_prev(0), m_mutex_velocity_cmd(), m_verbose(verbose) +{ } + +vpPololu::vpPololu(const std::string &device, int baudrate, int channel, bool verbose) + : m_channel(channel), m_apply_velocity_cmd(false), m_stop_velocity_cmd_thread(false), + m_vel_speed(1), m_vel_target_position(0), m_vel_speed_prev(1), m_vel_target_position_prev(0), m_mutex_velocity_cmd(), m_verbose(verbose) +{ + connect(device, baudrate, channel); +} + +void vpPololu::connect(const std::string &device, int baudrate, int channel) +{ + std::string error_msg; + m_channel = channel; + m_nb_servo++; + + if (!m_interface) { + if (m_verbose) { + std::cout << "Creating serial interface '" << device << "' at " << baudrate << " bauds" << std::endl; + } + m_interface = RPMSerialInterface::createSerialInterface(device, baudrate, &error_msg); + + if (m_interface == nullptr) { + throw(vpRobotException(vpRobotException::constructionError, + "Cannot connect to pololu board with serial port %s at baudrate %d.\n%s", + device.c_str(), baudrate, error_msg.c_str())); + + } + } + + std::thread t(&vpPololu::VelocityCmdThread, this); + t.detach(); +} + + +vpPololu::~vpPololu() +{ + m_stop_velocity_cmd_thread = true; + if (m_nb_servo == 1) { + if (m_verbose) { + std::cout << "Deleting serial interface" << std::endl; + } + delete m_interface; + } + m_nb_servo--; +} + +void vpPololu::calibrate(unsigned short &pwm_min, unsigned short &pwm_max) +{ + bool ret = false; + ret = m_interface->setTargetMSSCP(m_channel, 0); + if (ret == false) { + throw(vpException(vpException::fatalError, "Unable to set servo normalized target at min position")); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + pwm_min = getPwmPosition(); + + ret = m_interface->setTargetMSSCP(m_channel, 254); + if (ret == false) { + throw(vpException(vpException::fatalError, "Unable to set servo normalized target at max position")); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + pwm_max = getPwmPosition(); +} + +unsigned short vpPololu::radToPwm(float angle) const +{ + float a = m_range_pwm / m_range_angle; + float b = m_min_pwm - m_min_angle * a; + + return (a * angle + b); +} + +bool vpPololu::connected() const +{ + if (!m_interface->isOpen()) { + if (m_verbose) { + std::cout << "Serial Communication Failed!\n"; + } + return false; + } + return true; +} + +short vpPololu::radSToSpeed(float speed_rad_s) const +{ + return static_cast((speed_rad_s / 100.f) * (m_range_pwm / m_range_angle)); +} + +unsigned short vpPololu::getPwmPosition() const +{ + unsigned short position_pwm; + bool ret = false; + int nb_attempt = 10; + int wait_ms = 2; + for (int i = 0; i < nb_attempt && ret == false; i++) { + ret = m_interface->getPositionCP(m_channel, position_pwm); + if (ret == false) { + std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms)); + if (m_verbose) { + std::cout << "Failed to get position, new attempt: " << i << std::endl; + } + } + } + if (ret == false) { + throw(vpException(vpException::fatalError, "Unable to get servo position")); + } + return position_pwm; +} + +float vpPololu::getAngularPosition() const +{ + unsigned short position_pwm = getPwmPosition(); + float position_angle = pwmToRad(position_pwm); + + return position_angle; +} + +void vpPololu::getRangeAngles(float &minAngle, float &maxAngle) const +{ + minAngle = m_min_angle; + maxAngle = m_max_angle; +} + +void vpPololu::getRangePwm(unsigned short &min, unsigned short &max) +{ + min = m_min_pwm; + max = m_max_pwm; +} + +float vpPololu::pwmToRad(unsigned short pwm) const +{ + float a = m_range_angle / m_range_pwm; + float b = m_min_angle - m_min_pwm * a; + + return (a * pwm + b); +} + +void vpPololu::setAngularPosition(float pos_rad, float vel_rad_s) +{ + if ((m_min_angle <= pos_rad) && (pos_rad <= m_max_angle)) { + unsigned short pos_pwm = radToPwm(pos_rad); + unsigned short pos_speed = std::fabs(radSToSpeed(vel_rad_s)); + // Handle the case where pos_speed = 0 which corresponds to the pwm max speed + if (pos_speed == 0) { + pos_speed = 1; + } + setPwmPosition(pos_pwm, pos_speed); + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "Given position: %d is outside of the servo range. You can check the range using the method getRangeAngles()", pos_rad)); + } +} + +void vpPololu::setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm) +{ + bool ret = false; + if ((m_min_pwm <= pos_pwm) && (pos_pwm <= m_max_pwm)) { + if (speed_pwm <= 1000) { + ret = m_interface->setSpeedCP(m_channel, speed_pwm); + if (ret == false) { + throw(vpException(vpException::fatalError, "Unable to set servo speed")); + } + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "Given speed (pwm): %d is outside of the servo speed range. range is from 0 to 1000", speed_pwm)); + } + if (m_verbose) { + std::cout << "Channel " << m_channel << " set position (pwm): " << pos_pwm << " with speed: " << speed_pwm << std::endl; + } + ret = m_interface->setTargetCP(m_channel, pos_pwm); + if (ret == false) { + throw(vpException(vpException::fatalError, "Unable to set servo target position")); + } + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "Given position (pwm): %d is outside servo range. You can check the range using the method getRangePwm()", pos_pwm)); + } +} + +void vpPololu::setAngularVelocity(float vel_rad_s) +{ + short pwm_vel = radSToSpeed(vel_rad_s); + + // Handle the case where vel_rad_s != 0 but pwm_vel == 0. In that case we set a minimal speed + if (pwm_vel == 0) { + if (vel_rad_s > 0) { + pwm_vel = 1; + } + else if (vel_rad_s < 0) { + pwm_vel = -1; + } + } + setPwmVelocity(pwm_vel); +} + +void vpPololu::setPwmVelocity(short pwm_vel) +{ + unsigned short vel_speed; + unsigned short vel_target_position; + if (pwm_vel <= 1000) { + vel_speed = static_cast(std::abs(pwm_vel)); + if (pwm_vel > 0) { + vel_target_position = m_max_pwm; + } + else if (pwm_vel < 0) { + vel_target_position = m_min_pwm; + } + else { // pwm_vel = 0 + vel_target_position = getPwmPosition(); // Stay at current position + vel_speed = 1; // Set to min speed to keep current position + } + m_mutex_velocity_cmd.lock(); + m_vel_speed = vel_speed; + m_vel_target_position = vel_target_position; + m_apply_velocity_cmd = true; + m_mutex_velocity_cmd.unlock(); + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "Given pwm velocity %d is outside of the servo velocity range. Range is from 0 to 1000", pwm_vel)); + } +} + +float vpPololu::speedToRadS(short speed_pwm) const +{ + return (speed_pwm * 100) * (m_range_angle / m_range_pwm); +} + +void vpPololu::stopVelocityCmd() +{ + if (m_verbose) { + std::cout << "Stoping vel cmd channel: " << m_channel << std::endl; + } + + m_mutex_velocity_cmd.lock(); + m_apply_velocity_cmd = false; + m_mutex_velocity_cmd.unlock(); + + //std::this_thread::sleep_for(10 * millis); + unsigned short pos_pwm = getPwmPosition(); + if (m_verbose) { + std::cout << "Stoping channel " << m_channel << " at position " << pos_pwm << std::endl; + } + setPwmPosition(pos_pwm, 0); // 0 to be as fast as possible to reach pos_pwm +} + +void vpPololu::VelocityCmdThread() +{ + if (m_verbose) { + std::cout << "Create Velocity command thread" << std::endl; + } + unsigned short vel_speed; + unsigned short vel_target_position; + bool apply_velocity_cmd; + + while (!m_stop_velocity_cmd_thread) { + m_mutex_velocity_cmd.lock(); + vel_speed = m_vel_speed; + vel_target_position = m_vel_target_position; + apply_velocity_cmd = m_apply_velocity_cmd; + m_mutex_velocity_cmd.unlock(); + if (apply_velocity_cmd) { + //unsigned short position = getPwmPosition(); + //if (position != m_vel_target_position) { + if (m_vel_speed_prev != vel_speed || m_vel_target_position_prev != vel_target_position) { + setPwmPosition(vel_target_position, vel_speed); + } + + m_vel_speed_prev = vel_speed; + m_vel_target_position_prev = vel_target_position; + } + + std::this_thread::sleep_for(20 * millis); + } +} + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_robot.a(vpPololu.cpp.o) has no symbols +void dummy_vpPololu() { }; +#endif diff --git a/modules/robot/src/real-robot/pololu-maestro/vpRobotPololuPtu.cpp b/modules/robot/src/real-robot/pololu-maestro/vpRobotPololuPtu.cpp new file mode 100644 index 0000000000..f6dabef9e8 --- /dev/null +++ b/modules/robot/src/real-robot/pololu-maestro/vpRobotPololuPtu.cpp @@ -0,0 +1,297 @@ +/* + * 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: + * Common features for Pololu Maestro PanTiltUnit. + */ + +#include + + +#ifdef VISP_HAVE_POLOLU + +#include +#include +#include +#include + +vpRobotPololuPtu::vpRobotPololuPtu(const std::string &device, int baudrate, bool verbose) + : m_verbose(verbose) +{ + nDof = 2; + m_pan.connect(device, baudrate, 0); + m_pan.setPwmRange(4095, 7905); + m_pan.setAngularRange(vpMath::rad(-45), vpMath::rad(45)); + m_pan.setVerbose(verbose); + + m_tilt.connect(device, baudrate, 1); + m_tilt.setPwmRange(4095, 7905); + m_tilt.setAngularRange(vpMath::rad(-45), vpMath::rad(45)); + m_tilt.setVerbose(verbose); +} + +vpRobotPololuPtu::~vpRobotPololuPtu() +{ + m_pan.stopVelocityCmd(); + m_tilt.stopVelocityCmd(); +} + +void vpRobotPololuPtu::get_eJe(vpMatrix &eJe) +{ + vpColVector q(nDof); + getPosition(vpRobot::JOINT_STATE, q); + + get_eJe(q, eJe); +} + +void vpRobotPololuPtu::get_fJe(vpMatrix &fJe) +{ + vpColVector q(nDof); + getPosition(vpRobot::JOINT_STATE, q); + + get_fJe(q, fJe); +} + +void vpRobotPololuPtu::get_eJe(const vpColVector &q, vpMatrix &eJe) const +{ + eJe.resize(6, nDof); + + if (q.size() != static_cast(nDof)) { + throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector")); + } + + double s2 = sin(q[1]); + double c2 = cos(q[1]); + + eJe = 0; + + eJe[3][0] = -c2; + eJe[4][1] = -1; + eJe[5][0] = s2; +} + +void vpRobotPololuPtu::get_fJe(const vpColVector &q, vpMatrix &fJe) const +{ + if (q.size() != static_cast(nDof)) { + throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector")); + } + + fJe.resize(6, nDof); + + double s1 = sin(q[0]); + double c1 = cos(q[0]); + + fJe = 0; + + fJe[3][1] = s1; + fJe[4][1] = -c1; + fJe[5][0] = 1; +} + +float vpRobotPololuPtu::getAngularVelocityResolution() const +{ + return m_pan.speedToRadS(1); +} + +void vpRobotPololuPtu::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) +{ + if (q.size() != static_cast(nDof)) { + throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector")); + } + + if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) { + std::cout << "Warning: Robot is not in position-based control. Modification of the robot state" << std::endl; + setRobotState(vpRobot::STATE_POSITION_CONTROL); + } + + switch (frame) { + case vpRobot::CAMERA_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: " + "not implemented"); + break; + case vpRobot::REFERENCE_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: " + "not implemented"); + break; + case vpRobot::MIXT_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: " + "not implemented"); + break; + case vpRobot::END_EFFECTOR_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: " + "not implemented"); + break; + case vpRobot::JOINT_STATE: + break; + } + + float pos_vel = m_positioning_velocity_percentage * getMaxRotationVelocity(); + m_pan.setAngularPosition(q[0], pos_vel); + m_tilt.setAngularPosition(q[1], pos_vel); +} + +void vpRobotPololuPtu::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) +{ + if (q_dot.size() != static_cast(nDof)) { + throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector")); + } + + if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) { + throw vpRobotException(vpRobotException::wrongStateError, + "Cannot send a velocity to the robot " + "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) "); + } + + switch (frame) { + case vpRobot::CAMERA_FRAME: { + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the camera frame:" + "functionality not implemented"); + } + case vpRobot::JOINT_STATE: { + if (q_dot.getRows() != 2) { + throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector " + "in joint state"); + } + break; + } + case vpRobot::REFERENCE_FRAME: { + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the reference frame:" + "functionality not implemented"); + } + case vpRobot::MIXT_FRAME: { + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the mixt frame:" + "functionality not implemented"); + } + case vpRobot::END_EFFECTOR_FRAME: { + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the end-effector frame:" + "functionality not implemented"); + } + default: { + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "); + } + } + + bool norm = false; // Flag to indicate when velocities need to be normalized + + // Saturate joint speed + double max = getMaxRotationVelocity(); + vpColVector q_dot_sat(nDof); + + // Init q_dot_saturated + q_dot_sat = q_dot; + + for (unsigned int i = 0; i < static_cast(nDof); ++i) { // q1 and q2 + if (fabs(q_dot[i]) > max) { + norm = true; + max = fabs(q_dot[i]); + vpERROR_TRACE("Excess velocity: ROTATION " + "(axe nr.%d).", + i); + } + } + // Rotations velocities normalization + if (norm == true) { + max = getMaxRotationVelocity() / max; + q_dot_sat = q_dot * max; + } + + std::cout << "Send velocity: " << q_dot_sat.t() << std::endl; + + m_pan.setAngularVelocity(q_dot_sat[0]); + m_tilt.setAngularVelocity(q_dot_sat[1]); +} + +vpRobot::vpRobotStateType vpRobotPololuPtu::setRobotState(vpRobot::vpRobotStateType newState) +{ + switch (newState) { + case vpRobot::STATE_STOP: { + if (vpRobot::STATE_STOP != getRobotState()) { + stopVelocity(); + } + break; + } + case vpRobot::STATE_POSITION_CONTROL: { + if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { + std::cout << "Switch from velocity to position control." << std::endl; + stopVelocity(); + } + + break; + } + case vpRobot::STATE_VELOCITY_CONTROL: { + } + default: + break; + } + + return vpRobot::setRobotState(newState); +} + +void vpRobotPololuPtu::stopVelocity() +{ + m_pan.stopVelocityCmd(); + m_tilt.stopVelocityCmd(); +} + +void vpRobotPololuPtu::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) +{ + switch (frame) { + case vpRobot::CAMERA_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: " + "not implemented"); + break; + case vpRobot::REFERENCE_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: " + "not implemented"); + break; + case vpRobot::MIXT_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: " + "not implemented"); + break; + case vpRobot::END_EFFECTOR_FRAME: + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: " + "not implemented"); + break; + case vpRobot::JOINT_STATE: + break; + } + + q.resize(nDof); + q[0] = m_pan.getAngularPosition(); + q[1] = m_tilt.getAngularPosition(); +} + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_robot.a(vpRobotPololuPtu.cpp.o) has no symbols +void dummy_vpRobotPololuPtu() { }; +#endif diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp index 70cf55ab82..3881565ce2 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp @@ -46,7 +46,8 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include #include @@ -171,7 +172,8 @@ bool handleKeyboardInput(vpRobotMavsdk &drone, int key, bool &flying, double &la break; } vpTime::wait(40); // We wait 40ms to give the drone the time to process the command - } else { + } + else { running = false; } return running; @@ -186,35 +188,37 @@ int main(int argc, char **argv) if (std::string(argv[i]) == "--co" && i + 1 < argc) { opt_connecting_info = std::string(argv[i + 1]); i++; - } else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) { + } + else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) { std::cout << "\nUsage:\n" - << " " << argv[0] << "[--co ] [--help] [-h]\n" - << std::endl - << "Description:\n" - << " --co \n" - << " - UDP: udp://[host][:port]\n" - << " - TCP: tcp://[host][:port]\n" - << " - serial: serial://[path][:baudrate]\n" - << " - Default: udp://192.168.30.111:14552).\n\n" - << " For example, to connect to the simulator use URL: udp://:14552\n" - << " --help, -h\n" - << " Print help message.\n" - << std::endl; + << " " << argv[0] << "[--co ] [--help] [-h]\n" + << std::endl + << "Description:\n" + << " --co \n" + << " - UDP: udp://[host][:port]\n" + << " - TCP: tcp://[host][:port]\n" + << " - serial: serial://[path][:baudrate]\n" + << " - Default: udp://192.168.30.111:14552).\n\n" + << " For example, to connect to the simulator use URL: udp://:14552\n" + << " --help, -h\n" + << " Print help message.\n" + << std::endl; return EXIT_SUCCESS; - } else { + } + else { std::cout << "Error : unknown parameter " << argv[i] << std::endl - << "See " << argv[0] << " --help" << std::endl; + << "See " << argv[0] << " --help" << std::endl; return EXIT_SUCCESS; } } std::cout << std::endl - << "WARNING: this program does no sensing or avoiding of obstacles, " - << "the drone WILL collide with any objects in the way! Make sure the " - << "drone has approximately 3 meters of free space on all sides." << std::endl - << std::endl; + << "WARNING: this program does no sensing or avoiding of obstacles, " + << "the drone WILL collide with any objects in the way! Make sure the " + << "drone has approximately 3 meters of free space on all sides." << std::endl + << std::endl; - // Connect to the drone +// Connect to the drone vpRobotMavsdk drone(opt_connecting_info); if (drone.isRunning()) { @@ -229,10 +233,10 @@ int main(int argc, char **argv) vpKeyboard keyboard; std::cout << "\n| Control the drone with the keyboard :\n" - "| 't' to takeoff / 'l' to land / 'e' for emergency stop\n" - "| ('space','u','d','g') and ('i','k','j','l') to move\n" - "| 'q' to quit.\n" - << std::endl; + "| 't' to takeoff / 'l' to land / 'e' for emergency stop\n" + "| ('space','u','d','g') and ('i','k','j','l') to move\n" + "| 'q' to quit.\n" + << std::endl; while (running && drone.isRunning()) { @@ -244,11 +248,13 @@ int main(int argc, char **argv) } std::cout << "\nQuitting ...\n" << std::endl; - } else { + } + else { std::cout << "ERROR : failed to setup drone control." << std::endl; return EXIT_FAILURE; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "\nCaught an exception: " << e << std::endl; return EXIT_FAILURE; } @@ -260,13 +266,13 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp index 408b091c91..4b652acf43 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp @@ -47,18 +47,19 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include void usage(const std::string &bin_name) { std::cerr << "Usage : " << bin_name << " \n" - << "Connection URL format should be :\n" - << " - For TCP : tcp://[server_host][:server_port]\n" - << " - For UDP : udp://[bind_host][:bind_port]\n" - << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udp://:14540\n"; + << "Connection URL format should be :\n" + << " - For TCP : tcp://[server_host][:server_port]\n" + << " - For UDP : udp://[bind_host][:bind_port]\n" + << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udp://:14540\n"; } int main(int argc, char **argv) @@ -85,15 +86,15 @@ int main(int argc, char **argv) float ned_north, ned_east, ned_down, ned_yaw; drone.getPosition(ned_north, ned_east, ned_down, ned_yaw); std::cout << "Vehicle position in NED frame: " << ned_north << " " << ned_east << " " << ned_down << " [m] and " - << vpMath::deg(ned_yaw) << " [deg]" << std::endl; + << vpMath::deg(ned_yaw) << " [deg]" << std::endl; vpHomogeneousMatrix ned_M_frd; drone.getPosition(ned_M_frd); vpRxyzVector rxyz(ned_M_frd.getRotationMatrix()); std::cout << "Vehicle position in NED frame: " << ned_M_frd.getTranslationVector().t() << " [m] and " - << vpMath::deg(rxyz).t() << " [deg]" << std::endl; + << vpMath::deg(rxyz).t() << " [deg]" << std::endl; - // Set position in NED frame +// Set position in NED frame drone.setPositioningIncertitude(0.10, vpMath::rad(5.)); drone.setPosition(0.0, 1.0, ned_down, 0.0); // East @@ -113,13 +114,13 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp index a7debdebf4..519e82574c 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp @@ -47,18 +47,19 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include void usage(const std::string &bin_name) { std::cerr << "Usage : " << bin_name << " \n" - << "Connection URL format should be :\n" - << " - For TCP : tcp://[server_host][:server_port]\n" - << " - For UDP : udp://[bind_host][:bind_port]\n" - << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udp://:14540\n"; + << "Connection URL format should be :\n" + << " - For TCP : tcp://[server_host][:server_port]\n" + << " - For UDP : udp://[bind_host][:bind_port]\n" + << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udp://:14540\n"; } int main(int argc, char **argv) @@ -84,15 +85,15 @@ int main(int argc, char **argv) float ned_north, ned_east, ned_down, ned_yaw; drone.getPosition(ned_north, ned_east, ned_down, ned_yaw); std::cout << "Vehicle position in NED frame: " << ned_north << " " << ned_east << " " << ned_down << " [m] and " - << vpMath::deg(ned_yaw) << " [deg]" << std::endl; + << vpMath::deg(ned_yaw) << " [deg]" << std::endl; vpHomogeneousMatrix ned_M_frd; drone.getPosition(ned_M_frd); vpRxyzVector rxyz(ned_M_frd.getRotationMatrix()); std::cout << "Vehicle position in NED frame: " << ned_M_frd.getTranslationVector().t() << " [m] and " - << vpMath::deg(rxyz).t() << " [deg]" << std::endl; + << vpMath::deg(rxyz).t() << " [deg]" << std::endl; - // Set position in NED frame +// Set position in NED frame drone.setPositioningIncertitude(0.10, vpMath::rad(5.)); drone.setPositionRelative(0.0, 1.0, 0.0, 0.0); // Right @@ -110,13 +111,13 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp index baed14604f..c97e39649a 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp @@ -47,18 +47,19 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include void usage(const std::string &bin_name) { std::cerr << "Usage : " << bin_name << " \n" - << "Connection information format should be :\n" - << " - For TCP: tcp://[server_host][:server_port]\n" - << " - For UDP: udp://[bind_host][:bind_port]\n" - << " - For Serial: serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udp://:14540\n"; + << "Connection information format should be :\n" + << " - For TCP: tcp://[server_host][:server_port]\n" + << " - For UDP: udp://[bind_host][:bind_port]\n" + << " - For Serial: serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udp://:14540\n"; } int main(int argc, char **argv) @@ -75,8 +76,7 @@ int main(int argc, char **argv) drone.takeControl(); // Start off-board or guided mode // Drone takeoff - if (! drone.takeOff() ) - { + if (!drone.takeOff()) { std::cout << "Takeoff failed" << std::endl; return EXIT_FAILURE; } @@ -90,8 +90,8 @@ int main(int argc, char **argv) drone.getPosition(ned_M_frd); vpRxyzVector rxyz(ned_M_frd.getRotationMatrix()); std::cout << "Vehicle position in NED frame: " - << ned_M_frd.getTranslationVector().t() << " [m] and " - << vpMath::deg(rxyz).t() << " [deg]"<< std::endl; + << ned_M_frd.getTranslationVector().t() << " [m] and " + << vpMath::deg(rxyz).t() << " [deg]"<< std::endl; std::cout << "Hold position for 4 sec" << std::endl; drone.holdPosition(); @@ -109,13 +109,13 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp index f37fd5fa47..6c330c7058 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp @@ -47,7 +47,8 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include #include @@ -58,11 +59,11 @@ using std::this_thread::sleep_for; void usage(const std::string &bin_name) { std::cerr << "Usage : " << bin_name << " \n" - << "Connection URL format should be :\n" - << " - For TCP : tcp://[server_host][:server_port]\n" - << " - For UDP : udp://[bind_host][:bind_port]\n" - << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udp://:14540\n"; + << "Connection URL format should be :\n" + << " - For TCP : tcp://[server_host][:server_port]\n" + << " - For UDP : udp://[bind_host][:bind_port]\n" + << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udp://:14540\n"; } int main(int argc, char **argv) @@ -75,12 +76,11 @@ int main(int argc, char **argv) auto drone = vpRobotMavsdk(argv[1]); drone.setTakeOffAlt(.5); - if (! drone.takeOff() ) - { + if (!drone.takeOff()) { std::cout << "Takeoff failed" << std::endl; return EXIT_FAILURE; } - vpColVector vel_command{0.0, 0.0, 0.0, 0.0}; + vpColVector vel_command { 0.0, 0.0, 0.0, 0.0 }; drone.setForwardSpeed(0.3); std::cout << "Set forward speed of 0.3 m/s for 4 sec" << std::endl; @@ -106,13 +106,13 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp index a2cd332b8c..dffd649c22 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp @@ -47,7 +47,8 @@ #include -#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +// Check if std:c++17 or higher +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include #include @@ -58,11 +59,11 @@ using std::this_thread::sleep_for; void usage(const std::string &bin_name) { std::cerr << "Usage : " << bin_name << " \n" - << "Connection URL format should be :\n" - << " - For TCP : tcp://[server_host][:server_port]\n" - << " - For UDP : udp://[bind_host][:bind_port]\n" - << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udp://:14540\n"; + << "Connection URL format should be :\n" + << " - For TCP : tcp://[server_host][:server_port]\n" + << " - For UDP : udp://[bind_host][:bind_port]\n" + << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udp://:14540\n"; } int main(int argc, char **argv) @@ -74,8 +75,7 @@ int main(int argc, char **argv) auto robot = vpRobotMavsdk(argv[1]); - if (! robot.setGPSGlobalOrigin(48.117266, -1.6777926, 40.0)) - { + if (!robot.setGPSGlobalOrigin(48.117266, -1.6777926, 40.0)) { return EXIT_FAILURE; } @@ -83,15 +83,15 @@ int main(int argc, char **argv) robot.arm(); double delta_north = 1.; - double delta_east = 0.; - double delta_down = 0.; - double delta_yaw = 0.; + double delta_east = 0.; + double delta_down = 0.; + double delta_yaw = 0.; std::cout << "Move 1 meter north" << std::endl;; robot.setPositionRelative(delta_north, delta_east, delta_down, delta_yaw); - vpColVector frd_vel{0.0, 0.0, 0.0, 0.0}; - frd_vel[0]= -0.3; // forward vel m/s + vpColVector frd_vel { 0.0, 0.0, 0.0, 0.0 }; + frd_vel[0] = -0.3; // forward vel m/s //frd_vel[3]= vpMath::rad(10.); std::cout << "Go at 0.3m/s backward during 3 sec.\n"; @@ -99,15 +99,14 @@ int main(int argc, char **argv) vpTime::wait(3000); std::cout << "Go at 0.3m/s forward and rotate 10 deg/s along yaw during 2 sec.\n"; - frd_vel[0]= 0.3; // forward vel m/s - frd_vel[3]= vpMath::rad(10.); // yaw vel 10 deg/s converted in rad/s + frd_vel[0] = 0.3; // forward vel m/s + frd_vel[3] = vpMath::rad(10.); // yaw vel 10 deg/s converted in rad/s double t = vpTime::measureTimeMs(); do { vpTime::sleepMs(20); robot.setVelocity(frd_vel); - } - while(vpTime::measureTimeMs() - t < 2000.); // + } while (vpTime::measureTimeMs() - t < 2000.); // robot.disarm(); return EXIT_SUCCESS; @@ -119,13 +118,13 @@ int main() { #ifndef VISP_HAVE_MAVSDK std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + << std::endl; #endif -#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout - << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " - "rebuild ViSP.\n" - << std::endl; + << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and " + "rebuild ViSP.\n" + << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/robot/test/servo-pololu/testPololuPosition.cpp b/modules/robot/test/servo-pololu/testPololuPosition.cpp new file mode 100644 index 0000000000..0e4799f93f --- /dev/null +++ b/modules/robot/test/servo-pololu/testPololuPosition.cpp @@ -0,0 +1,238 @@ +/* + * 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: + * Common test for Pololu position control of one servo connected to a given channel. + */ + +/*! + * \example testPololuPosition.cpp + */ + +#include + +#include + +#ifdef VISP_HAVE_POLOLU + +#include +#include +#include +#include + +#include +#include + +void usage(const char **argv, int error, const std::string &device, int baudrate, int channel, + unsigned short pwm_min, unsigned short pwm_max, float angle_min, float angle_max) +{ + std::cout << "Synopsis" << std::endl + << " " << argv[0] << " [--device ] [--baud ] [--channel ] [--calibrate]" + << " [--range-pwm ] [--verbose, -v] [--help, -h]" << std::endl + << std::endl; + std::cout << "Description" << std::endl + << " --device Device name." << std::endl + << " Default: " << device << std::endl + << std::endl + << " --baud Serial link baud rate." << std::endl + << " Default: " << baudrate << std::endl + << std::endl + << " --channel Channel to dial with." << std::endl + << " Default: " << channel << std::endl + << std::endl + << " --range-pwm Set PWM min and max values." << std::endl + << " You can use \"--calibrate\" to retrieve min and max pwm values." + << " Default: " << pwm_min << " " << pwm_max << std::endl + << std::endl + << " --range-angles Set angle min and max values (deg)." << std::endl + << " Default: " << vpMath::deg(angle_min) << " " << vpMath::deg(angle_max) << std::endl + << std::endl + << " --verbose, -v Enable verbosity." << std::endl + << std::endl + << " --calibrate Start pwm calibration determining min and max admissible values." << std::endl + << " Once calibration done you can use \"--range-pwm \" option to set" << std::endl + << " the corresponding values" << std::endl + << std::endl + << " --help, -h Print this helper message." << std::endl + << std::endl; + if (error) { + std::cout << "Error" << std::endl + << " " + << "Unsupported parameter " << argv[error] << std::endl; + } +} + +int main(int argc, const char **argv) +{ +#ifdef _WIN32 + std::string opt_device = "COM4"; +#else + std::string opt_device = "/dev/ttyACM0"; + // Example for Mac OS, the Maestro creates two devices, use the one with the lowest number (the command port) + //std::string opt_device = "/dev/cu.usbmodem00031501"; +#endif + int opt_channel = 0; + int opt_baudrate = 38400; + bool opt_verbose = false; + bool opt_calibrate = false; + unsigned short opt_pwm_min = 4000; + unsigned short opt_pwm_max = 8000; + float opt_angle_min = vpMath::rad(-45); + float opt_angle_max = vpMath::rad(45); + float opt_positioning_velocity = vpMath::rad(10); + float last_angle = 0; + int time_s = 0; + + for (int i = 1; i < argc; i++) { + if (std::string(argv[i]) == "--device" && i + 1 < argc) { + opt_device = std::string(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--baud" && i + 1 < argc) { + opt_baudrate = std::atoi(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--channel" && i + 1 < argc) { + opt_channel = std::atoi(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--range-pwm" && i + 2 < argc) { + opt_pwm_min = static_cast(vpMath::rad(std::atoi(argv[i + 1]))); + opt_pwm_max = static_cast(vpMath::rad(std::atoi(argv[i + 2]))); + i += 2; + } + else if (std::string(argv[i]) == "--range-angles" && i + 2 < argc) { + opt_angle_min = static_cast(std::atof(argv[i + 1])); + opt_angle_max = static_cast(std::atof(argv[i + 2])); + i += 2; + } + else if (std::string(argv[i]) == "--calibrate") { + opt_calibrate = true; + } + else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { + opt_verbose = true; + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + usage(argv, 0, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max); + return EXIT_SUCCESS; + } + else { + usage(argv, i, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max); + return EXIT_FAILURE; + } + } + + try { + // Creating the servo object on channel 0 + vpPololu servo(opt_device, opt_baudrate, opt_channel, opt_verbose); + + std::cout << "Pololu board is " << (servo.connected() ? "connected" : "disconnected") << std::endl; + + if (opt_calibrate) { + std::cout << "Proceed to calibration to determine pwm min and max values..." << std::endl; + std::cout << "WARNING: Calibration will move the servo at channel " << opt_channel << "!" << std::endl; + std::cout << "Press Enter to move to min and max pwm positions..." << std::endl; + std::cin.ignore(); + + unsigned short pwm_min, pwm_max; + servo.calibrate(pwm_min, pwm_max); + std::cout << "Servo on channel " << opt_channel << " has pwm range [" << pwm_min << ", " << pwm_max << "]" << std::endl; + return EXIT_SUCCESS; + } + + servo.setPwmRange(opt_pwm_min, opt_pwm_max); + servo.setAngularRange(opt_angle_min, opt_angle_max); + + // Getting the ranges of the servo + servo.getRangePwm(opt_pwm_min, opt_pwm_max); + std::cout << "Position range (pwm): " << opt_pwm_min << " " << opt_pwm_max << std::endl; + servo.getRangeAngles(opt_angle_min, opt_angle_max); + std::cout << "Position range (deg): " << vpMath::deg(opt_angle_min) << " " << vpMath::deg(opt_angle_max) << std::endl; + + // Servo will first move to min pwm range wait 3 seconds and move to max pwm range + std::cout << "Move to min position (pwm): " << opt_pwm_min << " at max velocity" << std::endl; + servo.setPwmPosition(opt_pwm_min, 0); + std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "Servo reached position (pwm): " << servo.getPwmPosition() << std::endl; + + std::cout << "Move to max position (pwm): " << opt_pwm_max << " at max velocity" << std::endl; + servo.setPwmPosition(opt_pwm_max, 0); + std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "Servo reached position (pwm): " << servo.getPwmPosition() << std::endl; + + // Servo will first move to min angle wait 3 seconds and move to max angle + std::cout << "Move to min position (deg): " << vpMath::deg(opt_angle_min) << " at max velocity" << std::endl; + servo.setAngularPosition(opt_angle_min, 0); + std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "Servo reached position (deg): " << vpMath::deg(servo.getAngularPosition()) << std::endl; + + std::cout << "Move to max position (deg): " << vpMath::deg(opt_angle_max) << " at max velocity" << std::endl; + servo.setAngularPosition(opt_angle_max, 0); + std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "Servo reached position (deg): " << vpMath::deg(servo.getAngularPosition()) << std::endl; + + // Servo will move to 0 angle at a max velocity in rad/s + std::cout << "Move to zero position (deg): " << vpMath::deg(0) << " at max velocity" << std::endl; + servo.setAngularPosition(0, 0); + std::this_thread::sleep_for(std::chrono::seconds(3)); + last_angle = servo.getAngularPosition(); + std::cout << "Servo reached position (deg): " << vpMath::deg(last_angle) << std::endl; + + // Servo will first move to min angle at a given velocity in rad/s + std::cout << "Move to min position (deg): " << vpMath::deg(opt_angle_min) << " at " << vpMath::deg(opt_positioning_velocity) << " deg/s" << std::endl; + servo.setAngularPosition(opt_angle_min, opt_positioning_velocity); + // Estimate time to reach position + time_s = static_cast(std::abs((opt_angle_min - last_angle) / opt_positioning_velocity) + 2); + + std::this_thread::sleep_for(std::chrono::seconds(time_s)); + last_angle = servo.getAngularPosition(); + std::cout << "Servo reached position (deg): " << vpMath::deg(last_angle) << std::endl; + + std::cout << "Move to max position (deg): " << vpMath::deg(opt_angle_max) << " at " << vpMath::deg(opt_positioning_velocity) << " deg/s" << std::endl; + servo.setAngularPosition(opt_angle_max, opt_positioning_velocity); + // Estimate time to reach position + time_s = static_cast(std::abs((opt_angle_max - last_angle) / opt_positioning_velocity) + 2); + std::this_thread::sleep_for(std::chrono::seconds(time_s)); + last_angle = servo.getAngularPosition(); + std::cout << "Servo reached position (deg): " << vpMath::deg(last_angle) << std::endl; + + return EXIT_SUCCESS; + } + catch (const vpException &e) { + std::cout << e.getMessage() << std::endl; + return EXIT_FAILURE; + } +} + +#else +int main() +{ + std::cout << "ViSP doesn't support Pololu 3rd party library" << std::endl; +} +#endif diff --git a/modules/robot/test/servo-pololu/testPololuVelocity.cpp b/modules/robot/test/servo-pololu/testPololuVelocity.cpp new file mode 100644 index 0000000000..d9224a5f13 --- /dev/null +++ b/modules/robot/test/servo-pololu/testPololuVelocity.cpp @@ -0,0 +1,223 @@ +/* + * 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: + * Common test for Pololu velocity control of one servo connected to a given channel. + */ + +/*! + * \example testPololuVelocity.cpp + */ +#include + +#include + +#ifdef VISP_HAVE_POLOLU + +#include +#include +#include +#include + +#include +#include + +void usage(const char **argv, int error, const std::string &device, int baudrate, int channel, + unsigned short pwm_min, unsigned short pwm_max, float angle_min, float angle_max) +{ + std::cout << "Synopsis" << std::endl + << " " << argv[0] << " [--device ] [--channel ] [--calibrate] [--range-pwm ] [--verbose, -v] [--help, -h]" << std::endl + << std::endl; + std::cout << "Description" << std::endl + << " --device Device name." << std::endl + << " Default: " << device << std::endl + << std::endl + << " --baud Serial link baud rate." << std::endl + << " Default: " << baudrate << std::endl + << std::endl + << " --channel Channel to dial with." << std::endl + << " Default: " << channel << std::endl + << std::endl + << " --range-pwm Set PWM min and max values." << std::endl + << " You can use \"--calibrate\" to retrieve min and max pwm values." + << " Default: " << pwm_min << " " << pwm_max << std::endl + << std::endl + << " --range-angles Set angle min and max values (deg)." << std::endl + << " Default: " << vpMath::deg(angle_min) << " " << vpMath::deg(angle_max) << std::endl + << std::endl + << " --verbose, -v Enable verbosity." << std::endl + << std::endl + << " --calibrate Start pwm calibration determining min and max admissible values." << std::endl + << " Once calibration done you can use \"--range-pwm \" option to set" << std::endl + << " the corresponding values" << std::endl + << std::endl + << " --help, -h Print this helper message." << std::endl + << std::endl; + if (error) { + std::cout << "Error" << std::endl + << " " + << "Unsupported parameter " << argv[error] << std::endl; + } +} + +int main(int argc, const char **argv) +{ +#ifdef _WIN32 + std::string opt_device = "COM4"; +#else + std::string opt_device = "/dev/ttyACM0"; + // Example for Mac OS, the Maestro creates two devices, use the one with the lowest number (the command port) + //std::string opt_device = "/dev/cu.usbmodem00031501"; +#endif + int opt_channel = 0; + int opt_baudrate = 38400; + bool opt_verbose = false; + bool opt_calibrate = false; + unsigned short opt_pwm_min = 4000; + unsigned short opt_pwm_max = 8000; + float opt_angle_min = vpMath::rad(-45); + float opt_angle_max = vpMath::rad(45); + float opt_velocity = vpMath::rad(5); + float last_angle = 0; + + for (int i = 1; i < argc; i++) { + if (std::string(argv[i]) == "--device" && i + 1 < argc) { + opt_device = std::string(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--baud" && i + 1 < argc) { + opt_baudrate = std::atoi(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--channel" && i + 1 < argc) { + opt_channel = std::atoi(argv[i + 1]); + i++; + } + else if (std::string(argv[i]) == "--range-pwm" && i + 2 < argc) { + opt_pwm_min = static_cast(vpMath::rad(std::atoi(argv[i + 1]))); + opt_pwm_max = static_cast(vpMath::rad(std::atoi(argv[i + 2]))); + i += 2; + } + else if (std::string(argv[i]) == "--range-angles" && i + 2 < argc) { + opt_angle_min = static_cast(std::atof(argv[i + 1])); + opt_angle_max = static_cast(std::atof(argv[i + 2])); + i += 2; + } + else if (std::string(argv[i]) == "--calibrate") { + opt_calibrate = true; + } + else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { + opt_verbose = true; + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + usage(argv, 0, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max); + return EXIT_SUCCESS; + } + else { + usage(argv, i, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max); + return EXIT_FAILURE; + } + } + + std::chrono::seconds sec(1); + + try { + // Creating the servo object on channel 0 + vpPololu servo(opt_verbose); + + servo.connect(opt_device, opt_baudrate, opt_channel); + + if (opt_calibrate) { + std::cout << "Proceed to calibration to determine pwm min and max values..." << std::endl; + std::cout << "WARNING: Calibration will move the servo at channel " << opt_channel << "!" << std::endl; + std::cout << "Press Enter to move to min and max pwm positions..." << std::endl; + std::cin.ignore(); + + unsigned short pwm_min, pwm_max; + servo.calibrate(pwm_min, pwm_max); + std::cout << "Servo on channel " << opt_channel << " has pwm range [" << pwm_min << ", " << pwm_max << "]" << std::endl; + return EXIT_SUCCESS; + } + + servo.setPwmRange(opt_pwm_min, opt_pwm_max); + servo.setAngularRange(opt_angle_min, opt_angle_max); + + // Servo will move to 0 angle at a max velocity in rad/s + std::cout << "Move to zero position (deg): " << vpMath::deg(0) << " at max velocity" << std::endl; + servo.setAngularPosition(0, 0); + std::this_thread::sleep_for(std::chrono::seconds(3)); + last_angle = servo.getAngularPosition(); + std::cout << "Servo reached position (deg): " << vpMath::deg(last_angle) << std::endl; + + if (1) { + // Servo will first move in one direction at a velocity of 10 for 3 sec and move back in the other direction for 3 sec + short vel_pwm = 10; + std::cout << "Move at velocity (pwm): " << vel_pwm << " for 3 sec" << std::endl; + servo.setPwmVelocity(vel_pwm); + std::this_thread::sleep_for(3 * sec); + std::cout << "Servo reached position (pwm): " << servo.getPwmPosition() << std::endl; + + vel_pwm = -10; + std::cout << "Move at velocity (pwm): " << vel_pwm << " for 3 sec" << std::endl; + servo.setPwmVelocity(vel_pwm); + std::this_thread::sleep_for(3 * sec); + std::cout << "Servo reached position (pwm): " << servo.getPwmPosition() << std::endl; + std::cout << "End of velocity motion" << std::endl; + } + + // Servo will first move in one direction at a velocity of 10 for 3 sec and move back in the other direction for 3 sec + std::cout << "Move at velocity (deg/s): " << vpMath::deg(opt_velocity) << " for 3 sec" << std::endl; + servo.setAngularVelocity(opt_velocity); + std::this_thread::sleep_for(3 * sec); + std::cout << "Servo reached position (deg): " << servo.getPwmPosition() << std::endl; + + std::cout << "Move at velocity (deg/s): " << vpMath::deg(-opt_velocity) << " for 3 sec" << std::endl; + servo.setAngularVelocity(-opt_velocity); + std::this_thread::sleep_for(3 * sec); + std::cout << "Servo reached position (deg): " << servo.getPwmPosition() << std::endl; + + // Stopping the velocity command. + servo.stopVelocityCmd(); + + std::cout << "The end" << std::endl; + + return EXIT_SUCCESS; + } + catch (const vpException &e) { + std::cout << e.getMessage() << std::endl; + return EXIT_FAILURE; + } +} + +#else +int main() +{ + std::cout << "ViSP doesn't support Pololu 3rd party library" << std::endl; +} +#endif diff --git a/modules/sensor/CMakeLists.txt b/modules/sensor/CMakeLists.txt index 4529046650..be27453fca 100644 --- a/modules/sensor/CMakeLists.txt +++ b/modules/sensor/CMakeLists.txt @@ -99,22 +99,8 @@ if(USE_FTIITSDK) endif() if(USE_PCL) list(APPEND opt_incs ${PCL_INCLUDE_DIRS}) - - # list(APPEND opt_libs ${PCL_LIBRARIES}) - # Using PCL_LIBRARIES works to build visp library, examples, demos and test thanks to the components, - # but not tutorials when they are build outside ViSP as they are stand alone CMake projects that use - # ViSP as a 3rd party. - # To be clear PCL_LIBRARIES contains VTK 3rd party such as vtkalglib and not /usr/local/Cellar/vtk/6.3.0/lib/libvtkalglib-6.3.1.dylib - # full path as requested to use ViSP as 3rd party. This is the case for all VTK libraries that are PCL dependencies. - # The build of ViSP works with PCL_LIBRARIES since in that case thanks to vtkalglib properties, CMake - # is able to find the real name and location of the libraries. - # But when ViSP is used as a 3rd party where it should import PCL libraries, it doesn't work with - # PCL_LIBRARIES and especially with VTK_LIBRARIES. - # The solution here is to get the full location of VTK_LIBRARIES libraries thanks to the properties and link - # with these names. - # An other way could be to include PCLConfig.cmake, but in that case, visp-config and visp.pc - # will be not able to give the names of PCL libraries when used without CMake. - vp_find_pcl(PCL_LIBRARIES PCL_DEPS_INCLUDE_DIRS PCL_DEPS_LIBRARIES) + # To ensure to build with VTK and other PCL 3rd parties we are not using PCL_LIBRARIES but PCL_DEPS_INCLUDE_DIRS + # and PCL_DEPS_LIBRARIES instead list(APPEND opt_incs ${PCL_DEPS_INCLUDE_DIRS}) list(APPEND opt_libs ${PCL_DEPS_LIBRARIES}) endif() diff --git a/modules/tracker/mbt/CMakeLists.txt b/modules/tracker/mbt/CMakeLists.txt index 82d2106226..c5e5e7cb72 100644 --- a/modules/tracker/mbt/CMakeLists.txt +++ b/modules/tracker/mbt/CMakeLists.txt @@ -108,22 +108,8 @@ endif() if(USE_PCL) list(APPEND opt_incs ${PCL_INCLUDE_DIRS}) - - # list(APPEND opt_libs ${PCL_LIBRARIES}) - # Using PCL_LIBRARIES works to build visp library, examples, demos and test thanks to the components, - # but not tutorials when they are build outside ViSP as they are stand alone CMake projects that use - # ViSP as a 3rd party. - # To be clear PCL_LIBRARIES contains VTK 3rd party such as vtkalglib and not /usr/local/Cellar/vtk/6.3.0/lib/libvtkalglib-6.3.1.dylib - # full path as requested to use ViSP as 3rd party. This is the case for all VTK libraries that are PCL dependencies. - # The build of ViSP works with PCL_LIBRARIES since in that case thanks to vtkalglib properties, CMake - # is able to find the real name and location of the libraries. - # But when ViSP is used as a 3rd party where it should import PCL libraries, it doesn't work with - # PCL_LIBRARIES and especially with VTK_LIBRARIES. - # The solution here is to get the full location of VTK_LIBRARIES libraries thanks to the properties and link - # with these names. - # An other way could be to include PCLConfig.cmake, but in that case, visp-config and visp.pc - # will be not able to give the names of PCL libraries when used without CMake. - vp_find_pcl(PCL_LIBRARIES PCL_DEPS_INCLUDE_DIRS PCL_DEPS_LIBRARIES) + # To ensure to build with VTK and other PCL 3rd parties we are not using PCL_LIBRARIES but PCL_DEPS_INCLUDE_DIRS + # and PCL_DEPS_LIBRARIES instead list(APPEND opt_incs ${PCL_DEPS_INCLUDE_DIRS}) list(APPEND opt_libs ${PCL_DEPS_LIBRARIES}) endif() diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h index 7b6b694538..ed99ef4a17 100755 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h @@ -113,7 +113,8 @@ template class vpMbtTukeyEstimator #if HAVE_TRANSFORM namespace { -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) +// Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); }; #else template struct AbsDiff : public std::binary_function @@ -166,7 +167,8 @@ void vpMbtTukeyEstimator::MEstimator_impl(const std::vector &residues, std m_normres.resize(residues.size()); #if HAVE_TRANSFORM -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) +// Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med)); #else std::transform(residues.begin(), residues.end(), m_normres.begin(), @@ -270,7 +272,8 @@ inline void vpMbtTukeyEstimator::MEstimator_impl_simd(const std::vector< m_normres.resize(residues.size()); #if HAVE_TRANSFORM -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) +// Check if std:c++14 or higher +#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med)); #else std::transform(residues.begin(), residues.end(), m_normres.begin(), diff --git a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp index 1bbb006c38..29302d725d 100644 --- a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp +++ b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp @@ -407,6 +407,12 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, dou double Bti[4]; double dBti[4]; double d2Bti[4]; + + for (size_t i = 0; i < 4; ++i) { + Bti[i] = 0.; + dBti[i] = 0.; + d2Bti[i] = 0.; + } double *ptBti = &Bti[0]; double *ptdBti = &dBti[0]; diff --git a/modules/vision/include/visp3/vision/vpPlaneEstimation.h b/modules/vision/include/visp3/vision/vpPlaneEstimation.h index fbf7a4453f..a4a3152858 100644 --- a/modules/vision/include/visp3/vision/vpPlaneEstimation.h +++ b/modules/vision/include/visp3/vision/vpPlaneEstimation.h @@ -40,11 +40,9 @@ #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) - -// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910] -// Visual Studio: Structured bindings are available from Visual Studio 2017 version 15.3 [1911] (cf .cpp) +// Check if std:c++17 or higher. +// Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) // System #include diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h index fbc0101b5c..3c8a60589c 100644 --- a/modules/vision/include/visp3/vision/vpPose.h +++ b/modules/vision/include/visp3/vision/vpPose.h @@ -54,8 +54,9 @@ #include #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +// Check if std:c++17 or higher. +// Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #include #include #endif @@ -669,8 +670,9 @@ class VISP_EXPORT vpPose static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo); -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) + // Check if std:c++17 or higher. + // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) /*! * Compute the pose of a planar object from corresponding 2D-3D point coordinates and plane equation. diff --git a/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp b/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp index 0012c09412..8fa0e6b070 100644 --- a/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp +++ b/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp @@ -33,8 +33,8 @@ #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +// Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) // OpenMP #ifdef VISP_HAVE_OPENMP @@ -109,7 +109,7 @@ vpPlane estimatePlaneEquationSVD(const std::vector &point_cloud, vpColVe } // Compute centroid -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_17) +#if ((__cplusplus > 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG > 201703L))) auto [centroid, total_w] = compute_centroid(point_cloud, weights); #else // C++17 structured binding are not fully supported by clang 13.0 on macOS diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp index ff3857dadc..9a8b6c1c48 100644 --- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp +++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp @@ -248,8 +248,8 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) } } -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +// Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::optional vpPose::poseVirtualVSWithDepth(const std::vector &points, const vpHomogeneousMatrix &cMo) { auto residu_1 { 1e8 }, r { 1e8 - 1 }; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h index 0881c74a3e..6022914d71 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h @@ -42,6 +42,7 @@ #define _vpFeatureMoment_h_ #include +#include #include #include #include @@ -164,7 +165,7 @@ class VISP_EXPORT vpFeatureMoment : public vpBasicFeature double A; double B; double C; - char _name[255]; + std::string m_name; // private: //#ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -200,7 +201,7 @@ class VISP_EXPORT vpFeatureMoment : public vpBasicFeature vpFeatureMoment(vpMomentDatabase &data_base, double A_ = 0.0, double B_ = 0.0, double C_ = 0.0, vpFeatureMomentDatabase *featureMoments = nullptr, unsigned int nbmatrices = 1) : vpBasicFeature(), moment(nullptr), moments(data_base), featureMomentsDataBase(featureMoments), - interaction_matrices(nbmatrices), A(A_), B(B_), C(C_), _name() + interaction_matrices(nbmatrices), A(A_), B(B_), C(C_), m_name() { } /** @name Inherited functionalities from vpFeatureMoment */ @@ -221,11 +222,13 @@ class VISP_EXPORT vpFeatureMoment : public vpBasicFeature * Name of the moment corresponding to the feature. This allows to locate * the moment associated with the feature in the provided database. */ - virtual const char *momentName() const = 0; + virtual const std::string momentName() const = 0; + /*! * Name of the feature used to locate it in the database of features. */ - virtual const char *name() const = 0; + virtual const std::string name() const = 0; + void print(unsigned int select = FEATURE_ALL) const override; virtual void printDependencies(std::ostream &os) const; @@ -265,12 +268,12 @@ class VISP_EXPORT vpMomentGenericFeature : public vpFeatureMoment /*! * No specific moment name. */ - const char *momentName() const { return nullptr; } + const std::string momentName() const { return std::string(); } /*! * No specific feature name. */ - virtual const char *name() const { return nullptr; } + virtual const std::string name() const { return std::string(); } }; #endif diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h index 0c5966cf5d..eed6971d19 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h @@ -119,12 +119,12 @@ class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentAlpha"; } + const std::string momentName() const override { return "vpMomentAlpha"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentAlpha"; } + const std::string name() const override { return "vpFeatureMomentAlpha"; } vpColVector error(const vpBasicFeature &s_star, unsigned int select = FEATURE_ALL) override; }; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h index cee3586df5..b862c3182d 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h @@ -71,13 +71,13 @@ class VISP_EXPORT vpFeatureMomentArea : public vpFeatureMoment void compute_interaction() override; /*! - * associated moment name + * Associated moment name. */ - const char *momentName() const override { return "vpMomentArea"; } + const std::string momentName() const override { return "vpMomentArea"; } /*! - * feature name + * Feature name. */ - const char *name() const override { return "vpFeatureMomentArea"; } + const std::string name() const override { return "vpFeatureMomentArea"; } }; #endif diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h index dfaf6fc045..3e9e46fb07 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h @@ -99,12 +99,12 @@ class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentAreaNormalized"; } + const std::string momentName() const override { return "vpMomentAreaNormalized"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentAreaNormalized"; } + const std::string name() const override { return "vpFeatureMomentAreaNormalized"; } }; #else @@ -192,14 +192,14 @@ class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment void compute_interaction() override; /*! - * Associated moment name + * Associated moment name. */ - const char *momentName() const override { return "vpMomentAreaNormalized"; } + const std::string momentName() const override { return "vpMomentAreaNormalized"; } /*! - * Feature name + * Feature name. */ - const char *name() const override { return "vpFeatureMomentAreaNormalized"; } + const std::string name() const override { return "vpFeatureMomentAreaNormalized"; } }; #endif #endif diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h index 02195d8ea5..235c1249b7 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h @@ -96,11 +96,11 @@ class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentBasic"; } + const std::string momentName() const override { return "vpMomentBasic"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentBasic"; } + const std::string name() const override { return "vpFeatureMomentBasic"; } }; #endif diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h index 0f5b5da2de..ed01b28183 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h @@ -108,12 +108,12 @@ class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentCInvariant"; } + const std::string momentName() const override { return "vpMomentCInvariant"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentCInvariant"; } + const std::string name() const override { return "vpFeatureMomentCInvariant"; } /*! * Shortcut selector for \f$C_1\f$. @@ -246,11 +246,11 @@ class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentCInvariant"; } + const std::string momentName() const override { return "vpMomentCInvariant"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentCInvariant"; } + const std::string name() const override { return "vpFeatureMomentCInvariant"; } /*! * Shortcut selector for \f$C_1\f$. diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h index 8762be8960..1e3f957b68 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h @@ -100,12 +100,12 @@ class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment /*! * Associated moment name */ - const char *momentName() const override { return "vpMomentCentered"; } + const std::string momentName() const override { return "vpMomentCentered"; } /*! * Feature name */ - const char *name() const override { return "vpFeatureMomentCentered"; } + const std::string name() const override { return "vpFeatureMomentCentered"; } friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpFeatureMomentCentered &v); }; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h index 858f5b1a93..d6530fac82 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h @@ -42,6 +42,7 @@ #include #include #include +#include #include class vpFeatureMoment; @@ -154,11 +155,11 @@ class VISP_EXPORT vpFeatureMomentDatabase private: struct vpCmpStr_t { - bool operator()(const char *a, const char *b) const { return std::strcmp(a, b) < 0; } - char *operator=(const char *) { return nullptr; } // Only to avoid a warning under Visual with /Wall flag + bool operator()(const std::string &a, const std::string &b) const { return std::strcmp(a.c_str(), b.c_str()) < 0; } + std::string operator=(const std::string) { return std::string(); } // Only to avoid a warning under Visual with /Wall flag }; - std::map featureMomentsDataBase; - void add(vpFeatureMoment &featureMoment, char *name); + std::map featureMomentsDataBase; + void add(vpFeatureMoment &featureMoment, const std::string &name); public: /*! @@ -173,7 +174,7 @@ class VISP_EXPORT vpFeatureMomentDatabase virtual void updateAll(double A = 0.0, double B = 0.0, double C = 1.0); - vpFeatureMoment &get(const char *type, bool &found); + vpFeatureMoment &get(const std::string &feature_name, bool &found); // friend VISP_EXPORT std::ostream & operator<<(std::ostream& os, const // vpFeatureMomentDatabase& m); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h index 8958915f68..942ee4f6fd 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h @@ -166,12 +166,12 @@ class VISP_EXPORT vpFeatureMomentGravityCenter : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentGravityCenter"; } + const std::string momentName() const override { return "vpMomentGravityCenter"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentGravityCenter"; } + const std::string name() const override { return "vpFeatureMomentGravityCenter"; } /*! * Shortcut selector for \f$x_g\f$. @@ -238,12 +238,12 @@ class VISP_EXPORT vpFeatureMomentGravityCenter : public vpFeatureMoment /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentGravityCenter"; } + const std::string momentName() const override { return "vpMomentGravityCenter"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentGravityCenter"; } + const std::string name() const override { return "vpFeatureMomentGravityCenter"; } /*! * Shortcut selector for \f$x_g\f$. diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h index c75dc0d401..24e2a10480 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h @@ -106,12 +106,12 @@ class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMomen /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentGravityCenterNormalized"; } + const std::string momentName() const override { return "vpMomentGravityCenterNormalized"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentGravityCenterNormalized"; } + const std::string name() const override { return "vpFeatureMomentGravityCenterNormalized"; } /*! * Shortcut selector for \f$x_n\f$. @@ -260,12 +260,12 @@ class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMomen /*! * Associated moment name. */ - const char *momentName() const override { return "vpMomentGravityCenterNormalized"; } + const std::string momentName() const override { return "vpMomentGravityCenterNormalized"; } /*! * Feature name. */ - const char *name() const override { return "vpFeatureMomentGravityCenterNormalized"; } + const std::string name() const override { return "vpFeatureMomentGravityCenterNormalized"; } /*! * Shortcut selector for \f$x_n\f$. diff --git a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp index 490f4cc90f..bf8222a83b 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp @@ -103,9 +103,9 @@ void vpFeatureMoment::print(unsigned int select) const } /*! - Not implemented since visual representation of a moment doesn't often make - sense. -*/ + * Not implemented since visual representation of a moment doesn't often make + * sense. + */ void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { @@ -235,19 +235,14 @@ vpBasicFeature *vpFeatureMoment::duplicate() const * Links the feature to the feature's database. * * \note The feature's database is different from the moment's database. - * \param featureMoments : database in - * which the moment features are stored. + * \param featureMoments : database in which the moment features are stored. */ void vpFeatureMoment::linkTo(vpFeatureMomentDatabase &featureMoments) { - if (strlen(name()) >= 255) { - throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the moment name")); - } - - std::strcpy(_name, name()); + m_name = name(); this->featureMomentsDataBase = &featureMoments; - featureMoments.add(*this, _name); + featureMoments.add(*this, m_name); } void vpFeatureMoment::compute_interaction() { } diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp index 35a30a1aaf..58a8e2c6ec 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp @@ -43,23 +43,23 @@ * \param name : the feature's name, usually the string naming it's class. Each * name must be unique */ -void vpFeatureMomentDatabase::add(vpFeatureMoment &featureMoment, char *name) +void vpFeatureMomentDatabase::add(vpFeatureMoment &featureMoment, const std::string &name) { - featureMomentsDataBase.insert(std::pair((const char *)name, &featureMoment)); + featureMomentsDataBase.insert(std::pair(name, &featureMoment)); } /*! * Retrieves a moment feature from the database - * \param type : the name of the feature, the one specified when using add + * \param feature_name : The name of the feature, the one specified when using add * \param found : true if the type string is found inside the database, false * otherwise * * \return the moment feature corresponding to the type string */ -vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found) +vpFeatureMoment &vpFeatureMomentDatabase::get(const std::string &feature_name, bool &found) { - std::map::const_iterator it = - featureMomentsDataBase.find(type); + std::map::const_iterator it = + featureMomentsDataBase.find(feature_name); found = (it != featureMomentsDataBase.end()); return *(it->second); @@ -73,7 +73,7 @@ vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found) */ void vpFeatureMomentDatabase::updateAll(double A, double B, double C) { - std::map::const_iterator itr; + std::map::const_iterator itr; #ifdef VISP_HAVE_OPENMP std::vector values; values.reserve(featureMomentsDataBase.size()); diff --git a/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp b/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp index 86fa403e53..5dc9ef13b7 100644 --- a/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp +++ b/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp @@ -20,9 +20,8 @@ #include #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) && \ - defined(VISP_HAVE_DISPLAY) +// Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && defined(VISP_HAVE_DISPLAY) // Local helper namespace @@ -40,16 +39,16 @@ using Display = vpDisplayGTK; using Display = vpDisplayD3D; #endif -constexpr auto DispScaleType{vpDisplay::SCALE_AUTO}; +constexpr auto DispScaleType { vpDisplay::SCALE_AUTO }; // Model -constexpr auto ModelCommentHeader{"#"}; -constexpr auto ModelKeypointsHeader{"Keypoints"}; -constexpr auto ModelBoundsHeader{"Bounds"}; -constexpr auto ModelDataHeader{"data:"}; +constexpr auto ModelCommentHeader { "#" }; +constexpr auto ModelKeypointsHeader { "Keypoints" }; +constexpr auto ModelBoundsHeader { "Bounds" }; +constexpr auto ModelDataHeader { "data:" }; // Depth -constexpr auto DepthScale{0.001}; +constexpr auto DepthScale { 0.001 }; } // namespace #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -73,8 +72,8 @@ class Model std::map bounds(const vpHomogeneousMatrix &cMo = {}) const; private: - std::map m_keypoints{}; - std::map m_bounds{}; + std::map m_keypoints {}; + std::map m_bounds {}; }; inline Model::Model(const std::string &model_filename) @@ -82,27 +81,29 @@ inline Model::Model(const std::string &model_filename) std::fstream file; file.open(model_filename.c_str(), std::fstream::in); - std::string line{}, subs{}; - bool in_model_bounds{false}; - bool in_model_keypoints{false}; - unsigned int data_curr_line{0}; - unsigned int data_line_start_pos{0}; + std::string line {}, subs {}; + bool in_model_bounds { false }; + bool in_model_keypoints { false }; + unsigned int data_curr_line { 0 }; + unsigned int data_line_start_pos { 0 }; auto reset = [&]() { in_model_bounds = false; in_model_keypoints = false; data_curr_line = 0; data_line_start_pos = 0; - }; + }; while (getline(file, line)) { if (line.substr(0, std::string(ModelCommentHeader).size()) == ModelCommentHeader || line == ModelDataHeader) { continue; - } else if (line == ModelBoundsHeader) { + } + else if (line == ModelBoundsHeader) { reset(); in_model_bounds = true; continue; - } else if (line == ModelKeypointsHeader) { + } + else if (line == ModelKeypointsHeader) { reset(); in_model_keypoints = true; continue; @@ -116,18 +117,20 @@ inline Model::Model(const std::string &model_filename) try { std::stringstream ss(line.substr(data_line_start_pos, line.find("]") - data_line_start_pos)); unsigned int data_on_curr_line = 0; - vpColVector oXYZ({0, 0, 0, 1}); + vpColVector oXYZ({ 0, 0, 0, 1 }); while (getline(ss, subs, ',')) { oXYZ[data_on_curr_line++] = std::atof(subs.c_str()); } if (in_model_bounds) { m_bounds.try_emplace(data_curr_line, oXYZ[0], oXYZ[1], oXYZ[2]); - } else if (in_model_keypoints) { + } + else if (in_model_keypoints) { m_keypoints.try_emplace(data_curr_line, oXYZ[0], oXYZ[1], oXYZ[2]); } data_curr_line++; - } catch (...) { - // Line is empty or incomplete. We skeep it + } + catch (...) { + // Line is empty or incomplete. We skeep it } } @@ -156,20 +159,20 @@ std::ostream &operator<<(std::ostream &os, const Model &model) for (const auto &[id, bound] : model.bounds()) { // clang-format off os << std::setw(4) << std::setfill(' ') << id << ": " - << std::setw(6) << std::setfill(' ') << bound.get_X() << ", " - << std::setw(6) << std::setfill(' ') << bound.get_Y() << ", " - << std::setw(6) << std::setfill(' ') << bound.get_Z() << std::endl; - // clang-format on + << std::setw(6) << std::setfill(' ') << bound.get_X() << ", " + << std::setw(6) << std::setfill(' ') << bound.get_Y() << ", " + << std::setw(6) << std::setfill(' ') << bound.get_Z() << std::endl; + // clang-format on } os << "-Keypoints:" << std::endl; for (const auto &[id, keypoint] : model.keypoints()) { // clang-format off os << std::setw(4) << std::setfill(' ') << id << ": " - << std::setw(6) << std::setfill(' ') << keypoint.get_X() << ", " - << std::setw(6) << std::setfill(' ') << keypoint.get_Y() << ", " - << std::setw(6) << std::setfill(' ') << keypoint.get_Z() << std::endl; - // clang-format on + << std::setw(6) << std::setfill(' ') << keypoint.get_X() << ", " + << std::setw(6) << std::setfill(' ') << keypoint.get_Y() << ", " + << std::setw(6) << std::setfill(' ') << keypoint.get_Z() << std::endl; + // clang-format on } return os; @@ -190,11 +193,11 @@ readData(const std::string &input_directory, const unsigned int cpt = 0) const std::string filename_depth = buffer; // Read color - vpImage I_color{}; + vpImage I_color {}; vpImageIo::read(I_color, filename_color); // Read raw depth - vpImage I_depth_raw{}; + vpImage I_depth_raw {}; std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary); if (file_depth.is_open()) { unsigned int height = 0, width = 0; @@ -213,8 +216,8 @@ readData(const std::string &input_directory, const unsigned int cpt = 0) ss.str(""); ss << input_directory << "/camera.xml"; - vpXmlParserCamera parser{}; - vpCameraParameters color_param{}, depth_param{}; + vpXmlParserCamera parser {}; + vpCameraParameters color_param {}, depth_param {}; parser.parse(color_param, ss.str(), "color_camera", vpCameraParameters::perspectiveProjWithDistortion); parser.parse(depth_param, ss.str(), "depth_camera", vpCameraParameters::perspectiveProjWithDistortion); @@ -223,10 +226,10 @@ readData(const std::string &input_directory, const unsigned int cpt = 0) ss << input_directory << "/depth_M_color.txt"; std::ifstream file_depth_M_color(ss.str().c_str(), std::ios::in | std::ios::binary); - vpHomogeneousMatrix depth_M_color{}; + vpHomogeneousMatrix depth_M_color {}; depth_M_color.load(file_depth_M_color); - return {I_color, I_depth_raw, color_param, depth_param, depth_M_color}; + return { I_color, I_depth_raw, color_param, depth_param, depth_M_color }; } std::vector getRoiFromUser(vpImage color_img) @@ -236,11 +239,11 @@ std::vector getRoiFromUser(vpImage color_img) disp_color.display(color_img); disp_color.flush(color_img); - std::vector v_ip{}; + std::vector v_ip {}; do { // Prepare display disp_color.display(color_img); - auto disp_lane{0}; + auto disp_lane { 0 }; vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Select point along the d435 box boundary", vpColor::green); vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Left click to select a point", vpColor::green); @@ -255,8 +258,8 @@ std::vector getRoiFromUser(vpImage color_img) disp_color.flush(color_img); // Wait for new point - vpImagePoint ip{}; - vpMouseButton::vpMouseButtonType button{}; + vpImagePoint ip {}; + vpMouseButton::vpMouseButtonType button {}; vpDisplay::getClick(color_img, ip, button, true); switch (button) { @@ -288,14 +291,14 @@ std::map getKeypointsFromUser(vpImage color_img disp_color.display(color_img); disp_color.flush(color_img); - vpImage I_help{}; + vpImage I_help {}; vpImageIo::read(I_help, parent_data + "/data/d435_box_keypoints_user_helper.png"); Display disp_help(I_help, disp_color.getWindowXPosition() + color_img.getWidth(), disp_color.getWindowYPosition(), "Keypoints [help]", DispScaleType); disp_help.display(I_help); disp_help.flush(I_help); - std::map keypoints{}; + std::map keypoints {}; // - The next line produces an internal compiler error with Visual Studio 2017: // tutorial-pose-from-planar-object.cpp(304): fatal error C1001: An internal error has occurred in the compiler. // [C:\projects\visp\build\tutorial\computer-vision\tutorial-pose-from-planar-object.vcxproj] (compiler file @@ -309,7 +312,7 @@ std::map getKeypointsFromUser(vpImage color_img (void)ip_unused; // Prepare display disp_color.display(color_img); - auto disp_lane{0}; + auto disp_lane { 0 }; vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Select the keypoints " + Model::to_string(id), vpColor::green); @@ -323,7 +326,7 @@ std::map getKeypointsFromUser(vpImage color_img disp_color.flush(color_img); // Wait for new point - vpImagePoint ip{}; + vpImagePoint ip {}; vpDisplay::getClick(color_img, ip, true); keypoints.try_emplace(id, ip); } @@ -331,20 +334,18 @@ std::map getKeypointsFromUser(vpImage color_img return keypoints; } #endif // DOXYGEN_SHOULD_SKIP_THIS -#endif // #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= -// VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) && defined(VISP_HAVE_DISPLAY) +#endif int main(int, char *argv[]) { -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #if defined(VISP_HAVE_DISPLAY) // Get prior data //! [Prior_Data] auto [color_img, depth_raw, color_param, depth_param, depth_M_color] = - readData(vpIoTools::getParent(argv[0]) + "/data/d435_not_align_depth", 0); + readData(vpIoTools::getParent(argv[0]) + "/data/d435_not_align_depth", 0); const auto model = Model(vpIoTools::getParent(argv[0]) + "/data/d435_box.model"); //! [Prior_Data] @@ -358,7 +359,7 @@ int main(int, char *argv[]) display_color.display(color_img); display_color.flush(color_img); - vpImage depth_img{}; + vpImage depth_img {}; vpImageConvert::createDepthHistogram(depth_raw, depth_img); Display display_depth(depth_img, display_color.getWindowXPosition() + display_color.getWidth(), 0, "Depth", DispScaleType); @@ -367,29 +368,29 @@ int main(int, char *argv[]) // Ask roi for plane estimation //! [Roi_Plane_Estimation] - vpPolygon roi_color_img{}; + vpPolygon roi_color_img {}; roi_color_img.buildFrom(getRoiFromUser(color_img), true); - std::vector roi_corners_depth_img{}; + std::vector roi_corners_depth_img {}; std::transform( cbegin(roi_color_img.getCorners()), cend(roi_color_img.getCorners()), std::back_inserter(roi_corners_depth_img), std::bind((vpImagePoint(*)(const vpImage &, double, double, double, const vpCameraParameters &, const vpCameraParameters &, const vpHomogeneousMatrix &, const vpHomogeneousMatrix &, const vpImagePoint &)) & - vpColorDepthConversion::projectColorToDepth, + vpColorDepthConversion::projectColorToDepth, depth_raw, DepthScale, 0.1, 0.6, depth_param, color_param, depth_M_color.inverse(), depth_M_color, std::placeholders::_1)); - const vpPolygon roi_depth_img{roi_corners_depth_img}; + const vpPolygon roi_depth_img { roi_corners_depth_img }; //! [Roi_Plane_Estimation] vpDisplay::displayPolygon(depth_img, roi_depth_img.getCorners(), vpColor::green); display_depth.flush(depth_img); // Estimate the plane - vpImage heat_map{}; + vpImage heat_map {}; //! [Plane_Estimation] const auto obj_plane_in_depth = - vpPlaneEstimation::estimatePlane(depth_raw, DepthScale, depth_param, roi_depth_img, 1000, heat_map); + vpPlaneEstimation::estimatePlane(depth_raw, DepthScale, depth_param, roi_depth_img, 1000, heat_map); if (!obj_plane_in_depth) { return EXIT_FAILURE; } @@ -417,7 +418,7 @@ int main(int, char *argv[]) //! [Pose_Estimation] // Display the model - std::vector d435_box_bound{}; + std::vector d435_box_bound {}; // - The next line produces an internal compiler error with Visual Studio 2017: // tutorial-pose-from-planar-object.cpp(428): fatal error C1001: An internal error has occurred in the compiler. // [C:\projects\visp\build\tutorial\computer-vision\tutorial-pose-from-planar-object.vcxproj] (compiler file @@ -430,14 +431,14 @@ int main(int, char *argv[]) // for ([[maybe_unused]] const auto &[_, bound] : model.bounds(*cMo)) { for (const auto &[id_unused, bound] : model.bounds(*cMo)) { (void)id_unused; - vpImagePoint ip{}; + vpImagePoint ip {}; vpMeterPixelConversion::convertPoint(color_param, bound.get_x(), bound.get_y(), ip); d435_box_bound.push_back(ip); } vpDisplay::displayPolygon(color_img, d435_box_bound, vpColor::blue); for (const auto &[id, keypoint] : model.keypoints(*cMo)) { - vpImagePoint ip{}; + vpImagePoint ip {}; vpMeterPixelConversion::convertPoint(color_param, keypoint.get_x(), keypoint.get_y(), ip); vpDisplay::displayCross(color_img, ip, 15, vpColor::orange); vpDisplay::displayText(color_img, ip + vpImagePoint(10, 10), Model::to_string(id), vpColor::orange); @@ -448,7 +449,7 @@ int main(int, char *argv[]) vpDisplay::displayFrame(color_img, *cMo, color_param, 0.05, vpColor::none, 3); // Wait before exiting - auto disp_lane{0}; + auto disp_lane { 0 }; vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "D435 box boundary [from model]", vpColor::blue); vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Keypoints [from model]", vpColor::orange); vpDisplay::displayText(color_img, 15 * ++disp_lane, 15, "Click to quit", vpColor::green); @@ -463,8 +464,7 @@ int main(int, char *argv[]) #else (void)argv; std::cout << "c++17 should be enabled to run this tutorial." << std::endl; -#endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= - // VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) +#endif return EXIT_SUCCESS; } diff --git a/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp b/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp index a52634f795..f8eeeb869b 100644 --- a/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp +++ b/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp @@ -64,9 +64,11 @@ std::string getAvailableDetectionContainer() return availableContainers; } -int main(int argc, const char *argv []) +int main(int argc, const char *argv[]) { -#if defined(HAVE_OPENCV_DNN) && defined(HAVE_OPENCV_VIDEOIO) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + // Check if std:c++17 or higher +#if defined(HAVE_OPENCV_DNN) && defined(HAVE_OPENCV_VIDEOIO) && \ + ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) try { std::string opt_device("0"); //! [OpenCV DNN face detector] diff --git a/tutorial/image/drawingHelpers.cpp b/tutorial/image/drawingHelpers.cpp index 98602dc10c..5cff95008e 100644 --- a/tutorial/image/drawingHelpers.cpp +++ b/tutorial/image/drawingHelpers.cpp @@ -33,37 +33,75 @@ #include #if defined(VISP_HAVE_X11) -vpDisplayX drawingHelpers::d; +vpDisplayX drawingHelpers::d_Iinput; +vpDisplayX drawingHelpers::d_dIx; +vpDisplayX drawingHelpers::d_dIy; +vpDisplayX drawingHelpers::d_IcannyVisp; +vpDisplayX drawingHelpers::d_IcannyImgFilter; #elif defined(HAVE_OPENCV_HIGHGUI) -vpDisplayOpenCV drawingHelpers::d; +vpDisplayOpenCV drawingHelpers::d_Iinput; +vpDisplayOpenCV drawingHelpers::d_dIx; +vpDisplayOpenCV drawingHelpers::d_dIy; +vpDisplayOpenCV drawingHelpers::d_IcannyVisp; +vpDisplayOpenCV drawingHelpers::d_IcannyImgFilter; #elif defined(VISP_HAVE_GTK) -vpDisplayGTK drawingHelpers::d; +vpDisplayGTK drawingHelpers::d_Iinput; +vpDisplayGTK drawingHelpers::d_dIx; +vpDisplayGTK drawingHelpers::d_dIy; +vpDisplayGTK drawingHelpers::d_IcannyVisp; +vpDisplayGTK drawingHelpers::d_IcannyImgFilter; #elif defined(VISP_HAVE_GDI) -vpDisplayGDI drawingHelpers::d; +vpDisplayGDI drawingHelpers::d_Iinput; +vpDisplayGDI drawingHelpers::d_dIx; +vpDisplayGDI drawingHelpers::d_dIy; +vpDisplayGDI drawingHelpers::d_IcannyVisp; +vpDisplayGDI drawingHelpers::d_IcannyImgFilter; #elif defined(VISP_HAVE_D3D9) -vpDisplayD3D drawingHelpers::d; +vpDisplayD3D drawingHelpers::d_Iinput; +vpDisplayD3D drawingHelpers::d_dIx; +vpDisplayD3D drawingHelpers::d_dIy; +vpDisplayD3D drawingHelpers::d_IcannyVisp; +vpDisplayD3D drawingHelpers::d_IcannyImgFilter; #endif -vpImage drawingHelpers::I_disp; - -bool drawingHelpers::display(vpImage &I, const std::string &title, const bool &blockingMode) +void drawingHelpers::init(vpImage &Iinput, vpImage &IcannyVisp, vpImage *p_dIx, + vpImage *p_dIy, vpImage *p_IcannyimgFilter) { - I_disp = I; #if defined(VISP_HAVE_DISPLAY) - if (!d.isInitialised()) { - d.init(I_disp); - vpDisplay::setTitle(I_disp, title); + d_Iinput.init(Iinput, 10, 10); + d_IcannyVisp.init(IcannyVisp, 10, Iinput.getHeight() + 10 * 2); + if (p_dIx != nullptr) { + d_dIx.init(*p_dIx, Iinput.getWidth() + 2 * 10, 10); + } + if (p_dIy != nullptr) { + d_dIy.init(*p_dIy, 2 * Iinput.getWidth() + 3 * 10, 10); + } + if (p_IcannyimgFilter != nullptr) { + d_IcannyImgFilter.init(*p_IcannyimgFilter, Iinput.getWidth() + 2 * 10, Iinput.getHeight() + 10 * 2); } #else - (void)title; + (void)Iinput; + (void)IcannyVisp; + (void)p_dIx; + (void)p_dIy; + (void)p_IcannyimgFilter; #endif +} - vpDisplay::display(I_disp); - vpDisplay::displayText(I_disp, 15, 15, "Left click to continue...", vpColor::red); - vpDisplay::displayText(I_disp, 35, 15, "Right click to stop...", vpColor::red); - vpDisplay::flush(I_disp); +void drawingHelpers::display(vpImage &I, const std::string &title) +{ + vpDisplay::display(I); + vpDisplay::setTitle(I, title); + vpDisplay::flush(I); +} + +bool drawingHelpers::waitForClick(const vpImage &I, const bool &blockingMode) +{ + vpDisplay::displayText(I, 15, 15, "Left click to continue...", vpColor::red); + vpDisplay::displayText(I, 35, 15, "Right click to stop...", vpColor::red); + vpDisplay::flush(I); vpMouseButton::vpMouseButtonType button; - vpDisplay::getClick(I_disp, button, blockingMode); + vpDisplay::getClick(I, button, blockingMode); bool hasToContinue = true; if (button == vpMouseButton::button3) { // Right click => stop the program @@ -72,24 +110,3 @@ bool drawingHelpers::display(vpImage &I, const std::string &title, const return hasToContinue; } - -bool drawingHelpers::display(vpImage &D, const std::string &title, const bool &blockingMode) -{ - vpImage I; // Image to display - vpImageConvert::convert(D, I); - return display(I, title, blockingMode); -} - -bool drawingHelpers::display(vpImage &D, const std::string &title, const bool &blockingMode) -{ - vpImage I; // Image to display - vpImageConvert::convert(D, I); - return display(I, title, blockingMode); -} - -bool drawingHelpers::display(vpImage &F, const std::string &title, const bool &blockingMode) -{ - vpImage I; // Image to display - vpImageConvert::convert(F, I); - return display(I, title, blockingMode); -} diff --git a/tutorial/image/drawingHelpers.h b/tutorial/image/drawingHelpers.h index e612b20237..cc2619554a 100644 --- a/tutorial/image/drawingHelpers.h +++ b/tutorial/image/drawingHelpers.h @@ -38,66 +38,67 @@ namespace drawingHelpers { #if defined(VISP_HAVE_X11) -extern vpDisplayX d; +extern vpDisplayX d_Iinput; +extern vpDisplayX d_dIx; +extern vpDisplayX d_dIy; +extern vpDisplayX d_IcannyVisp; +extern vpDisplayX d_IcannyImgFilter; #elif defined(HAVE_OPENCV_HIGHGUI) -extern vpDisplayOpenCV d; +extern vpDisplayOpenCV d_Iinput; +extern vpDisplayOpenCV d_dIx; +extern vpDisplayOpenCV d_dIy; +extern vpDisplayOpenCV d_IcannyVisp; +extern vpDisplayOpenCV d_IcannyImgFilter; #elif defined(VISP_HAVE_GTK) -extern vpDisplayGTK d; +extern vpDisplayGTK d_Iinput; +extern vpDisplayGTK d_dIx; +extern vpDisplayGTK d_dIy; +extern vpDisplayGTK d_IcannyVisp; +extern vpDisplayGTK d_IcannyImgFilter; #elif defined(VISP_HAVE_GDI) -extern vpDisplayGDI d; +extern vpDisplayGDI d_Iinput; +extern vpDisplayGDI d_dIx; +extern vpDisplayGDI d_dIy; +extern vpDisplayGDI d_IcannyVisp; +extern vpDisplayGDI d_IcannyImgFilter; #elif defined(VISP_HAVE_D3D9) -extern vpDisplayD3D d; +extern vpDisplayD3D d_Iinput; +extern vpDisplayD3D d_dIx; +extern vpDisplayD3D d_dIy; +extern vpDisplayD3D d_IcannyVisp; +extern vpDisplayD3D d_IcannyImgFilter; #endif -extern vpImage I_disp; /*!< Displayed image.*/ - /** - * \brief Display a RGB image and catch the user clicks to know if - * the user wants to stop the program. + * \brief Initialize the different displays. * - * \param[out] I The RGB image to display. - * \param[in] title The title of the window. - * \param[in] blockingMode If true, wait for a click to switch to the next image. - * \return true The user wants to continue the application. - * \return false The user wants to stop the application. + * \param[out] Iinput Input image of the program. + * \param[out] IcannyVisp Image resulting from the vpCannyEdgeDetection method. + * \param[out] p_dIx If different from nullptr, pointer towards the gradient along the horizontal axis. + * \param[out] p_dIy If different from nullptr, pointer towards the gradient along the vertical axis. + * \param[out] p_IcannyimgFilter If different from nullptr, pointer towards the result of the vpImageFilter::canny + * method. */ -bool display(vpImage &I, const std::string &title, const bool &blockingMode); +void init(vpImage &Iinput, vpImage &IcannyVisp, vpImage *p_dIx, + vpImage *p_dIy, vpImage *p_IcannyimgFilter); /** - * \brief Display a gray-scale image and catch the user clicks to know if - * the user wants to stop the program. + * \brief Display a gray-scale image. * * \param[out] I The gray-scale image to display. * \param[in] title The title of the window. - * \param[in] blockingMode If true, wait for a click to switch to the next image. - * \return true The user wants to continue the application. - * \return false The user wants to stop the application. */ -bool display(vpImage &I, const std::string &title, const bool &blockingMode); +void display(vpImage &I, const std::string &title); /** - * \brief Display a double precision image and catch the user clicks to know if - * the user wants to stop the program. + * \brief Catch the user clicks to know if the user wants to stop the program. * - * \param[out] D The double precision image to display. - * \param[in] title The title of the window. - * \param[in] blockingMode If true, wait for a click to switch to the next image. - * \return true The user wants to continue the application. - * \return false The user wants to stop the application. - */ -bool display(vpImage &D, const std::string &title, const bool &blockingMode); - -/** - * \brief Display a floating-point precision image and catch the user clicks to know if - * the user wants to stop the program. - * - * \param[out] F The floating-point precision image to display. - * \param[in] title The title of the window. + * \param[in] I The gray-scale image to display. * \param[in] blockingMode If true, wait for a click to switch to the next image. * \return true The user wants to continue the application. * \return false The user wants to stop the application. */ -bool display(vpImage &F, const std::string &title, const bool &blockingMode); +bool waitForClick(const vpImage &I, const bool &blockingMode); } #endif diff --git a/tutorial/image/tutorial-canny.cpp b/tutorial/image/tutorial-canny.cpp index 2e74f9ac36..c5ece751ae 100644 --- a/tutorial/image/tutorial-canny.cpp +++ b/tutorial/image/tutorial-canny.cpp @@ -66,8 +66,11 @@ void computeMeanMaxStdev(const vpImage &I, float &mean, float &max, float &st stdev = std::sqrt(stdev); } -void setGradientOutsideClass(const vpImage &I, const int &gaussianKernelSize, const float &gaussianStdev, vpCannyEdgeDetection &cannyDetector, - const unsigned int apertureSize, const vpImageFilter::vpCannyFilteringAndGradientType &filteringType) +void setGradientOutsideClass(const vpImage &I, const int &gaussianKernelSize, const float &gaussianStdev, + vpCannyEdgeDetection &cannyDetector, const unsigned int apertureSize, + const vpImageFilter::vpCannyFilteringAndGradientType &filteringType, + vpImage &dIx_uchar, vpImage &dIy_uchar +) { // Computing the gradients vpImage dIx, dIy; @@ -82,11 +85,13 @@ void setGradientOutsideClass(const vpImage &I, const int &gaussia computeMeanMaxStdev(dIx, mean, max, stdev); std::string title = "Gradient along the horizontal axis. Mean = " + std::to_string(mean) + "+/-" + std::to_string(stdev) + " Max = " + std::to_string(max); - drawingHelpers::display(dIx, title, true); + vpImageConvert::convert(dIx, dIx_uchar); + drawingHelpers::display(dIx_uchar, title); computeMeanMaxStdev(dIy, mean, max, stdev); title = "Gradient along the horizontal axis. Mean = " + std::to_string(mean) + "+/-" + std::to_string(stdev) + " Max = " + std::to_string(max); - drawingHelpers::display(dIy, title, true); + vpImageConvert::convert(dIy, dIy_uchar); + drawingHelpers::display(dIy_uchar, title); } void usage(const std::string &softName, int gaussianKernelSize, float gaussianStdev, float lowerThresh, float upperThresh, @@ -222,7 +227,7 @@ int main(int argc, const char *argv[]) vpCannyEdgeDetection cannyDetector(opt_gaussianKernelSize, opt_gaussianStdev, opt_apertureSize, opt_lowerThresh, opt_upperThresh, opt_lowerThreshRatio, opt_upperThreshRatio, opt_filteringType); - vpImage I_canny_input; + vpImage I_canny_input, I_canny_visp, dIx_uchar, dIy_uchar, I_canny_imgFilter; if (!opt_img.empty()) { // Detection on the user image vpImageIo::read(I_canny_input, opt_img); @@ -237,24 +242,41 @@ int main(int argc, const char *argv[]) } } + // Initialization of the displays + I_canny_visp = I_canny_imgFilter = dIx_uchar = dIy_uchar = I_canny_input; + vpImage *p_dIx = nullptr, *p_dIy = nullptr, *p_IcannyImgFilter = nullptr; + + if (opt_gradientOutsideClass) { + p_dIx = &dIx_uchar; + p_dIy = &dIy_uchar; + } + + if (opt_useVpImageFilterCanny) { + p_IcannyImgFilter = &I_canny_imgFilter; + } + drawingHelpers::init(I_canny_input, I_canny_visp, p_dIx, p_dIy, p_IcannyImgFilter); + + // Computing the gradient outside the vpCannyEdgeDetection class if asked if (opt_gradientOutsideClass) { - setGradientOutsideClass(I_canny_input, opt_gaussianKernelSize, opt_gaussianStdev, cannyDetector, opt_apertureSize, opt_filteringType); + setGradientOutsideClass(I_canny_input, opt_gaussianKernelSize, opt_gaussianStdev, cannyDetector, opt_apertureSize, + opt_filteringType, dIx_uchar, dIy_uchar); } - vpImage I_canny = cannyDetector.detect(I_canny_input); + I_canny_visp = cannyDetector.detect(I_canny_input); float mean, max, stdev; computeMeanMaxStdev(I_canny_input, mean, max, stdev); std::string title("Input of the Canny edge detector. Mean = " + std::to_string(mean) + "+/-" + std::to_string(stdev) + " Max = " + std::to_string(max)); - drawingHelpers::display(I_canny_input, title, true); - drawingHelpers::display(I_canny, "Canny results on image " + opt_img, true); + drawingHelpers::display(I_canny_input, title); + drawingHelpers::display(I_canny_visp, "Canny results on image " + opt_img); if (opt_useVpImageFilterCanny) { float cannyThresh = opt_upperThresh; float lowerThresh(opt_lowerThresh); - vpImageFilter::canny(I_canny_input, I_canny, opt_gaussianKernelSize, lowerThresh, cannyThresh, + vpImageFilter::canny(I_canny_input, I_canny_imgFilter, opt_gaussianKernelSize, lowerThresh, cannyThresh, opt_apertureSize, opt_gaussianStdev, opt_lowerThreshRatio, opt_upperThreshRatio, true, opt_backend, opt_filteringType); - drawingHelpers::display(I_canny, "Canny results with \"" + vpImageFilter::vpCannyBackendTypeToString(opt_backend) + "\" backend", true); + drawingHelpers::display(I_canny_imgFilter, "Canny results with \"" + vpImageFilter::vpCannyBackendTypeToString(opt_backend) + "\" backend"); } + drawingHelpers::waitForClick(I_canny_input, true); return EXIT_SUCCESS; } diff --git a/tutorial/munkres/tutorial-munkres-assignment.cpp b/tutorial/munkres/tutorial-munkres-assignment.cpp index 3dbe9cf8d2..712d2440b7 100644 --- a/tutorial/munkres/tutorial-munkres-assignment.cpp +++ b/tutorial/munkres/tutorial-munkres-assignment.cpp @@ -19,8 +19,8 @@ int main() { -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ - (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) + // Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) #if defined(VISP_HAVE_DISPLAY) // Create base img @@ -28,8 +28,8 @@ int main() // Generate random points //! [Rand_Img_Pts] - vpUniRand rand{}; - std::vector rand_ips{}; + vpUniRand rand {}; + std::vector rand_ips {}; while (rand_ips.size() < 10) { rand_ips.emplace_back(rand.uniform(10, I.getHeight() - 10), rand.uniform(10, I.getWidth() - 10)); } @@ -56,11 +56,11 @@ int main() // Local helper to display a point in the image auto display_point = [&I](const vpImagePoint &ip, const vpColor &color) { I.display->displayCircle(ip, 5, color, true, 1); - }; + }; vpDisplay::display(I); - auto disp_lane{0}; + auto disp_lane { 0 }; vpDisplay::displayText(I, 15 * ++disp_lane, 15, "Left click to add a point", vpColor::black); vpDisplay::displayText(I, 15 * ++disp_lane, 15, "Middle click to continue (run Munkres)", vpColor::black); vpDisplay::displayText(I, 15 * ++disp_lane, 15, "Right click to quit", vpColor::black); @@ -70,14 +70,15 @@ int main() // Ask user to clic on point //! [User_Img_Pts] - std::vector user_ips{}; - vpMouseButton::vpMouseButtonType button{}; + std::vector user_ips {}; + vpMouseButton::vpMouseButtonType button {}; while (button != vpMouseButton::button2) { - vpImagePoint ip{}; + vpImagePoint ip {}; vpDisplay::getClick(I, ip, button, true); if (button == vpMouseButton::button1) { user_ips.push_back(ip); - } else if (button == vpMouseButton::button3) { + } + else if (button == vpMouseButton::button3) { return EXIT_SUCCESS; } @@ -112,7 +113,8 @@ int main() vpDisplay::flush(I); vpDisplay::getClick(I); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; } #endif // defined(VISP_HAVE_DISPLAY) diff --git a/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp b/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp index e418b1eb3c..0fff291973 100644 --- a/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp +++ b/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp @@ -2,8 +2,11 @@ #include #include -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_DNN) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(HAVE_OPENCV_HIGHGUI)) + +// Check if std:c++17 or higher +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \ + defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && \ + defined(HAVE_OPENCV_DNN) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(HAVE_OPENCV_HIGHGUI)) #include @@ -188,7 +191,7 @@ NLOHMANN_JSON_SERIALIZE_ENUM(DetectionMethod, { ); -int main(int argc, const char *argv []) +int main(int argc, const char *argv[]) { unsigned width = 640, height = 480; vpCameraParameters cam; @@ -279,7 +282,8 @@ int main(int argc, const char *argv []) vpDisplayOpenCV d; #endif //d.setDownScalingFactor(vpDisplay::SCALE_AUTO); -#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && \ + ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) vpDetectorDNNOpenCV::DNNResultsParsingType detectorType = vpDetectorDNNOpenCV::dnnResultsParsingTypeFromString(detectorTypeString); vpDetectorDNNOpenCV::NetConfig netConfig(detectorConfidenceThreshold, detectorNmsThreshold, labels, @@ -367,7 +371,8 @@ int main(int argc, const char *argv []) if (!initialized) { tracking = false; std::optional detection = std::nullopt; -#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && \ + ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) if (detectionMethod == DetectionMethod::DNN) { detection = detectObjectForInitMegaposeDnn( dnn, frame, objectName, initialized ? std::optional(megaposeEstimate) : std::nullopt); diff --git a/tutorial/visual-servo/ibvs/ibvs_settings.json b/tutorial/visual-servo/ibvs/ibvs_settings.json index 8e077e3de2..e2254fd6a1 100644 --- a/tutorial/visual-servo/ibvs/ibvs_settings.json +++ b/tutorial/visual-servo/ibvs/ibvs_settings.json @@ -4,15 +4,15 @@ "errorThreshold": 0.0001, "interactionMatrix": "current", "cdMo": { - "cols": 6, - "rows": 1, + "cols": 1, + "rows": 6, "type": "vpPoseVector", "data": [0.0, 0.0, 0.75, 0.0, 0.0, 0.0] }, "cMo": { - "cols": 6, - "rows": 1, + "cols": 1, + "rows": 6, "type": "vpPoseVector", "data": [0.3, 0.3, 1.2, 0.349066, 0.349066, 1.5708] } -} \ No newline at end of file +} diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp index 406f860664..7c91180d54 100644 --- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp +++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp @@ -266,7 +266,7 @@ int main(int argc, char *argv[]) unsigned int iter = 0; bool end = false; - std::cout << "Start visual-servoing loop until convergence..." << std::endl; + std::cout << "Starting visual-servoing loop until convergence..." << std::endl; //! [VS loop] while (!end) { robot.getPosition(wMc);