diff --git a/.vscode/settings.json b/.vscode/settings.json index 2a31aeaf5f..3abc6d4e03 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -114,7 +114,8 @@ "numbers": "cpp", "semaphore": "cpp", "stop_token": "cpp", - "view": "cpp" + "view": "cpp", + "mixinvector": "cpp" }, "C_Cpp.vcFormat.indent.namespaceContents": false, "editor.formatOnSave": true, @@ -132,5 +133,6 @@ "C_Cpp.vcFormat.space.aroundBinaryOperator": "ignore", "editor.comments.insertSpace": true, "C_Cpp.vcFormat.indent.preserveComments": true, - "C_Cpp.doxygen.generatedStyle": "/*!" + "C_Cpp.doxygen.generatedStyle": "/*!", + "files.insertFinalNewline": true } diff --git a/3rdparty/qbdevice/commands.h b/3rdparty/qbdevice/commands.h index 7bfd694e7f..25ec1a8bd8 100644 --- a/3rdparty/qbdevice/commands.h +++ b/3rdparty/qbdevice/commands.h @@ -36,7 +36,7 @@ * \details * This file is included in the qbMove and qbHand firmware, in its libraries and * applications. It contains all definitions that are necessary for the - * contruction of communication packages. + * construction of communication packages. * * It includes definitions for all of the device commands, parameters and also * the size of answer packages. @@ -96,7 +96,7 @@ enum qbmove_command CMD_GET_CURR_AND_MEAS = 134, ///< Command for asking device's /// measurements and currents CMD_SET_POS_STIFF = 135, ///< Not used in the softhand firmware - CMD_GET_EMG = 136, ///< Command for asking device's emg sensors + CMD_GET_EMG = 136, ///< Command for asking device's emg sensors /// measurements CMD_GET_VELOCITIES = 137, ///< Command for asking device's /// velocity measurements @@ -104,12 +104,12 @@ enum qbmove_command /// (mostly used for debugging sent commands) CMD_GET_ACCEL = 139, ///< Command for asking device's /// acceleration measurements - CMD_GET_CURR_DIFF = 140, ///< Command for asking device's + CMD_GET_CURR_DIFF = 140, ///< Command for asking device's /// current difference between a measured /// one and an estimated one (Only for SoftHand) CMD_SET_CURR_DIFF = 141, ///< Command used to set current difference modality /// (Only for Cuff device) - CMD_SET_CUFF_INPUTS = 142, ///< Command used to set Cuff device inputs + CMD_SET_CUFF_INPUTS = 142, ///< Command used to set Cuff device inputs /// (Only for Cuff device) CMD_SET_WATCHDOG = 143, ///< Command for setting watchdog timer /// or disable it @@ -163,7 +163,7 @@ enum qbmove_parameter PARAM_DOUBLE_ENC_ON_OFF = 19, ///< Double Encoder Y/N PARAM_MOT_HANDLE_RATIO = 20, ///< Multiplier between handle and motor PARAM_MOTOR_SUPPLY = 21, ///< Motor supply voltage of the hand - PARAM_CURRENT_LOOKUP = 23, ///< Table of values used to calculate + PARAM_CURRENT_LOOKUP = 23, ///< Table of values used to calculate /// an estimated current of the SoftHand PARAM_DL_POS_PID = 24, ///< Double loop position PID PARAM_DL_CURR_PID = 25 ///< Double loop current PID @@ -213,7 +213,7 @@ enum qbmove_control_mode { CONTROL_CURRENT = 2, ///< Current control CURR_AND_POS_CONTROL = 3, ///< Position and current control DEFLECTION_CONTROL = 4, ///< Deflection control - DEFL_CURRENT_CONTROL = 5 ///< Deflection and current control + DEFL_CURRENT_CONTROL = 5 ///< Deflection and current control }; diff --git a/CMakeLists.txt b/CMakeLists.txt index 646b40f269..2d18f91bf4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,7 +137,7 @@ endif() # VISP version number. An even minor number corresponds to releases. set(VISP_VERSION_MAJOR "3") set(VISP_VERSION_MINOR "6") -set(VISP_VERSION_PATCH "0") +set(VISP_VERSION_PATCH "1") set(VISP_VERSION "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}") set(VISP_SOVERSION "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}") # Package revision number diff --git a/ChangeLog.txt b/ChangeLog.txt index c61a29eb7b..143a9f1bcc 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -5,6 +5,18 @@ Copyright (C) 2005 - 2023 by Inria. All rights reserved. https://visp.inria.fr +ViSP 3.x.x (Version in development) + - Contributors: + . Fabien Spindler, + - New classes + . + - New features and improvements + . + - Tutorials + . + - Bug fixed + . +---------------------------------------------- ViSP 3.6.0 (released September 22, 2023) - Contributors: . Fabien Spindler, Souriya Trinh, Romain Lagneau, Antonio Marino, Samuel Felton, diff --git a/appveyor.yml b/appveyor.yml index 4f6b23b0be..13430ea240 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # Specify version format -version: "3.6.0-{build}" +version: "3.6.1-{build}" image: - Visual Studio 2017 diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index b2e49cdc13..bb4dffd702 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -878,7 +878,7 @@ EXCLUDE = "@VISP_SOURCE_DIR@/tutorial/ios/StartedImageProc/Starte "@VISP_SOURCE_DIR@/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.h" \ "@VISP_SOURCE_DIR@/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera/VispDetector.h" \ "@VISP_SOURCE_DIR@/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera/VispDetector.mm" \ - "@VISP_SOURCE_DIR@/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay+withContext.h" + "@VISP_SOURCE_DIR@/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h" # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp index 3559126e6c..898de9e557 100644 --- a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp +++ b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp @@ -133,14 +133,14 @@ bool getOptions(int argc, char **argv, unsigned int &deviceCount, bool &saveVide // Code adapted from the original author Dan Mašek to be compatible with ViSP // image -class FrameQueue +class vpFrameQueue { public: - struct cancelled + struct vpCancelled_t { }; - FrameQueue() + vpFrameQueue() : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits::max()), m_mutex() { } @@ -173,13 +173,13 @@ class FrameQueue while (m_queueColor.empty()) { if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } m_cond.wait(lock); if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } } @@ -199,11 +199,11 @@ class FrameQueue std::mutex m_mutex; }; -class StorageWorker +class vpStorageWorker { public: - StorageWorker(FrameQueue &queue, const std::string &filename, unsigned int width, unsigned int height) + vpStorageWorker(vpFrameQueue &queue, const std::string &filename, unsigned int width, unsigned int height) : m_queue(queue), m_filename(filename), m_width(width), m_height(height) { } @@ -227,18 +227,18 @@ class StorageWorker } } } - catch (FrameQueue::cancelled &) { + catch (vpFrameQueue::vpCancelled_t &) { } } private: - FrameQueue &m_queue; + vpFrameQueue &m_queue; std::string m_filename; unsigned int m_width; unsigned int m_height; }; -class ShareImage +class vpShareImage { private: @@ -249,15 +249,15 @@ class ShareImage unsigned int m_totalSize; public: - struct cancelled + struct vpCancelled_t { }; - ShareImage() : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), m_totalSize(0) { } + vpShareImage() : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), m_totalSize(0) { } - virtual ~ShareImage() + virtual ~vpShareImage() { if (m_pImgData != NULL) { - delete [] m_pImgData; + delete[] m_pImgData; } } @@ -274,13 +274,13 @@ class ShareImage std::unique_lock lock(m_mutex); if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } m_cond.wait(lock); if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } // Copy to imageData @@ -307,7 +307,7 @@ class ShareImage m_totalSize = totalSize; if (m_pImgData != NULL) { - delete [] m_pImgData; + delete[] m_pImgData; } m_pImgData = new unsigned char[m_totalSize]; @@ -320,7 +320,7 @@ class ShareImage } }; -void capture(vpV4l2Grabber *const pGrabber, ShareImage &share_image) +void capture(vpV4l2Grabber *const pGrabber, vpShareImage &share_image) { vpImage local_img; @@ -340,7 +340,7 @@ void capture(vpV4l2Grabber *const pGrabber, ShareImage &share_image) } void display(unsigned int width, unsigned int height, int win_x, int win_y, unsigned int deviceId, - ShareImage &share_image, FrameQueue &queue, bool save) + vpShareImage &share_image, vpFrameQueue &queue, bool save) { vpImage local_img(height, width); @@ -419,7 +419,7 @@ void display(unsigned int width, unsigned int height, int win_x, int win_y, unsi } } } - catch (ShareImage::cancelled &) { + catch (vpShareImage::vpCancelled_t &) { std::cout << "Cancelled!" << std::endl; } @@ -428,7 +428,7 @@ void display(unsigned int width, unsigned int height, int win_x, int win_y, unsi } // Namespace -int main(int argc, char *argv []) +int main(int argc, char *argv[]) { unsigned int deviceCount = 1; unsigned int cameraScale = 1; // 640x480 @@ -459,13 +459,13 @@ int main(int argc, char *argv []) std::cout << "Grabbers: " << grabbers.size() << std::endl; - std::vector share_images(grabbers.size()); + std::vector share_images(grabbers.size()); std::vector capture_threads; std::vector display_threads; // Synchronized queues for each camera stream - std::vector save_queues(grabbers.size()); - std::vector storages; + std::vector save_queues(grabbers.size()); + std::vector storages; std::vector storage_threads; std::string parent_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); @@ -495,7 +495,7 @@ int main(int argc, char *argv []) if (saveVideo) { for (auto &s : storages) { // Start the storage thread for the current camera stream - storage_threads.emplace_back(&StorageWorker::run, &s); + storage_threads.emplace_back(&vpStorageWorker::run, &s); } } diff --git a/example/device/framegrabber/saveRealSenseData.cpp b/example/device/framegrabber/saveRealSenseData.cpp index e384ce1c18..4b8155b29f 100644 --- a/example/device/framegrabber/saveRealSenseData.cpp +++ b/example/device/framegrabber/saveRealSenseData.cpp @@ -182,13 +182,13 @@ bool getOptions(int argc, char **argv, bool &save, std::string &output_directory return true; } -class FrameQueue +class vpFrameQueue { public: - struct cancelled + struct vpCancelled_t { }; - FrameQueue() + vpFrameQueue() : m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(), m_maxQueueSize(1024 * 8), m_mutex() { } @@ -252,13 +252,13 @@ class FrameQueue while (m_queueColor.empty() || m_queueDepth.empty() || m_queuePointCloud.empty() || m_queueInfrared.empty()) { if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } m_cond.wait(lock); if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } } @@ -290,10 +290,10 @@ class FrameQueue std::mutex m_mutex; }; -class StorageWorker +class vpStorageWorker { public: - StorageWorker(FrameQueue &queue, const std::string &directory, bool save_color, bool save_depth, bool save_pointcloud, + vpStorageWorker(vpFrameQueue &queue, const std::string &directory, bool save_color, bool save_depth, bool save_pointcloud, bool save_infrared, bool save_pointcloud_binary_format, int #ifndef VISP_HAVE_PCL @@ -437,13 +437,13 @@ class StorageWorker } } } - catch (const FrameQueue::cancelled &) { - std::cout << "Receive cancel FrameQueue." << std::endl; + catch (const vpFrameQueue::vpCancelled_t &) { + std::cout << "Receive cancel vpFrameQueue." << std::endl; } } private: - FrameQueue &m_queue; + vpFrameQueue &m_queue; std::string m_directory; unsigned int m_cpt; bool m_save_color; @@ -607,10 +607,10 @@ int main(int argc, char *argv[]) file.close(); } - FrameQueue save_queue; - StorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud, + vpFrameQueue save_queue; + vpStorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud, save_infrared, save_pointcloud_binary_format, width, height); - std::thread storage_thread(&StorageWorker::run, &storage); + std::thread storage_thread(&vpStorageWorker::run, &storage); #ifdef USE_REALSENSE2 rs2::align align_to(RS2_STREAM_COLOR); diff --git a/example/reflex-takktile/yaml/motor_constants.yaml b/example/reflex-takktile/yaml/motor_constants.yaml index 0fda19a757..a20c8be37b 100644 --- a/example/reflex-takktile/yaml/motor_constants.yaml +++ b/example/reflex-takktile/yaml/motor_constants.yaml @@ -1,3 +1,3 @@ # Motor information that stays relatively constant motor_to_joint_inverted: [1, -1, 1, -1] -motor_to_joint_gear_ratio: [1.42, 1.42, 1.42, 1.5] \ No newline at end of file +motor_to_joint_gear_ratio: [1.42, 1.42, 1.42, 1.5] diff --git a/example/servo-bebop2/servoBebop2.cpp b/example/servo-bebop2/servoBebop2.cpp index c7c61693dc..f95f8d053e 100644 --- a/example/servo-bebop2/servoBebop2.cpp +++ b/example/servo-bebop2/servoBebop2.cpp @@ -432,7 +432,7 @@ int main(int argc, char **argv) mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d mg.compute(); // Compute gravity center moment mc.compute(); // Compute centered moments AFTER gravity center - man.setDesiredArea(area); // Desired area was init at 0 (unknow at contruction), need to be updated here + man.setDesiredArea(area); // Desired area was init at 0 (unknow at construction), need to be updated here man.compute(); // Compute area normalized moment AFTER centered moment mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment diff --git a/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp b/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp index c5c9b7a96f..f7d14d15ae 100644 --- a/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp +++ b/example/servo-pixhawk/servoPixhawkDroneIBVS.cpp @@ -411,7 +411,7 @@ int main(int argc, char **argv) mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d mg.compute(); // Compute gravity center moment mc.compute(); // Compute centered moments AFTER gravity center - man.setDesiredArea(area); // Desired area was init at 0 (unknow at contruction), + man.setDesiredArea(area); // Desired area was init at 0 (unknow at construction), // need to be updated here man.compute(); // Compute area normalized moment AFTER centered moment mgn.compute(); // Compute gravity center normalized moment AFTER area normalized diff --git a/example/tracking/mbtGenericTrackingDepth.cpp b/example/tracking/mbtGenericTrackingDepth.cpp index 6189d7cfa2..32dbb3cd4e 100644 --- a/example/tracking/mbtGenericTrackingDepth.cpp +++ b/example/tracking/mbtGenericTrackingDepth.cpp @@ -236,7 +236,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co return true; } -struct rs_intrinsics +struct vpRealsenseIntrinsics_t { float ppx; /**< Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ @@ -249,7 +249,7 @@ struct rs_intrinsics float coeffs[5]; /**< Distortion coefficients */ }; -void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth) +void rs_deproject_pixel_to_point(float point[3], const vpRealsenseIntrinsics_t &intrin, const float pixel[2], float depth) { float x = (pixel[0] - intrin.ppx) / intrin.fx; float y = (pixel[1] - intrin.ppy) / intrin.fy; @@ -324,7 +324,7 @@ bool read_data(unsigned int cpt, const std::string &input_directory, vpImage. - - The code below shows how to create a 3-element column vector of doubles, set the element values and access them: - \code - #include = VISP_CXX_STANDARD_11) - vpColVector v({-1, -2.1, -3}); - std::cout << "v:\n" << v << std::endl; - #endif - } - \endcode - The vector could also be initialized using operator=(const std::initializer_list< double > &) - \code - int main() - { - #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - vpColVector v; - v = {-1, -2.1, -3}; - #endif - } - \endcode - - JSON serialization - - Since ViSP 3.6.0, if ViSP is build with \ref soft_tool_json 3rd-party we introduce JSON serialization capabilities for vpColVector. - The following sample code shows how to save a pose vector in a file named `col-vector.json` - and reload the values from this JSON file. - \code - #include - - int main() - { - #if defined(VISP_HAVE_NLOHMANN_JSON) - std::string filename = "col-vector.json"; - { - vpColVector v({ 1, 2, 3, 4 }); - std::ofstream file(filename); - const nlohmann::json j = v; - file << j; - file.close(); - } - { - std::ifstream file(filename); - const nlohmann::json j = nlohmann::json::parse(file); - vpColVector v; - v = j.get(); - file.close(); - std::cout << "Read homogeneous matrix from " << filename << ":\n" << v.t() << std::endl; - } - #endif - } - \endcode - If you build and execute the sample code, it will produce the following output: - \code{.unparsed} - Read homogeneous matrix from col-vector.json: - 1 2 3 4 - \endcode - - The content of the `pose-vector.json` file is the following: - \code{.unparsed} - $ cat col-vector.json - {"cols":1,"data":[1.0,2.0,3.0,4.0],"rows":4,"type":"vpColVector"} - \endcode -*/ + * \class vpColVector + * \ingroup group_core_matrices + * + * \brief Implementation of column vector and the associated operations. + * + * This class provides a data structure for a column vector that contains + * values of double. It contains also some functions to achieve a set of + * operations on these vectors. + * + * The vpColVector class is derived from vpArray2D. + * + * The code below shows how to create a 3-element column vector of doubles, set the element values and access them: + * \code + * #include = VISP_CXX_STANDARD_11) + * vpColVector v({-1, -2.1, -3}); + * std::cout << "v:\n" << v << std::endl; + * #endif + * } + * \endcode + * The vector could also be initialized using operator=(const std::initializer_list< double > &) + * \code + * int main() + * { + * #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + * vpColVector v; + * v = {-1, -2.1, -3}; + * #endif + * } + * \endcode + * + * JSON serialization + * + * Since ViSP 3.6.0, if ViSP is build with \ref soft_tool_json 3rd-party we introduce JSON serialization capabilities for vpColVector. + * The following sample code shows how to save a pose vector in a file named `col-vector.json` + * and reload the values from this JSON file. + * \code + * #include + * + * int main() + * { + * #if defined(VISP_HAVE_NLOHMANN_JSON) + * std::string filename = "col-vector.json"; + * { + * vpColVector v({ 1, 2, 3, 4 }); + * std::ofstream file(filename); + * const nlohmann::json j = v; + * file << j; + * file.close(); + * } + * { + * std::ifstream file(filename); + * const nlohmann::json j = nlohmann::json::parse(file); + * vpColVector v; + * v = j.get(); + * file.close(); + * std::cout << "Read homogeneous matrix from " << filename << ":\n" << v.t() << std::endl; + * } + * #endif + * } + * \endcode + * If you build and execute the sample code, it will produce the following output: + * \code{.unparsed} + * Read homogeneous matrix from col-vector.json: + * 1 2 3 4 + * \endcode + * + * The content of the `pose-vector.json` file is the following: + * \code{.unparsed} + * $ cat col-vector.json + * {"cols":1,"data":[1.0,2.0,3.0,4.0],"rows":4,"type":"vpColVector"} + * \endcode + */ class VISP_EXPORT vpColVector : public vpArray2D { friend class vpMatrix; public: - //! Basic constructor that creates an empty 0-size column vector. + /*! + * Basic constructor that creates an empty 0-size column vector. + */ vpColVector() : vpArray2D() { } - //! Construct a column vector of size n. \warning Elements are not - //! initialized. If you want to set an initial value use - //! vpColVector(unsigned int, double). + + /*! + * Construct a column vector of size n. + * \warning Elements are not initialized. If you want to set an initial value use + * vpColVector(unsigned int, double). + */ explicit vpColVector(unsigned int n) : vpArray2D(n, 1) { } - //! Construct a column vector of size n. Each element is set to \e val. + + /*! + * Construct a column vector of size n. Each element is set to \e val. + */ vpColVector(unsigned int n, double val) : vpArray2D(n, 1, val) { } - //! Copy constructor that allows to construct a column vector from an other - //! one. + + /*! + * Copy constructor that allows to construct a column vector from an other one. + */ vpColVector(const vpColVector &v) : vpArray2D(v) { } + + /*! + * Construct a column vector from a part of an input column vector \e v. + * + * \param v : Input column vector used for initialization. + * \param r : row index in \e v that corresponds to the first element of the + * column vector to construct. + * \param nrows : Number of rows of the constructed + * column vector. + * + * The sub-vector starting from v[r] element and ending on v[r+nrows-1] element + * is used to initialize the constructed column vector. + * + * \sa init() + */ vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows); - //! Constructor that initialize a column vector from a 3-dim (Euler or - //! \f$\theta {\bf u}\f$) or 4-dim (quaternion) rotation vector. + + /*! + * Constructor that initialize a column vector from a 3-dim (Euler or + * \f$\theta {\bf u}\f$) or 4-dim (quaternion) rotation vector. + */ vpColVector(const vpRotationVector &v); - //! Constructor that initialize a column vector from a 6-dim pose vector. + + /*! + * Constructor that initialize a column vector from a 6-dim pose vector. + */ vpColVector(const vpPoseVector &p); - //! Constructor that initialize a column vector from a 3-dim translation - //! vector. + + /*! + * Constructor that initialize a column vector from a 3-dim translation vector. + */ vpColVector(const vpTranslationVector &t); + + /*! + * Constructor that creates a column vector from a m-by-1 matrix `M`. + * + * \exception vpException::dimensionError If the matrix is not a m-by-1 + * matrix. + */ vpColVector(const vpMatrix &M); + + /*! + * Constructor that takes column `j` of matrix `M`. + */ vpColVector(const vpMatrix &M, unsigned int j); + + /*! + * Constructor that creates a column vector from a std vector of double. + */ vpColVector(const std::vector &v); + + /*! + * Constructor that creates a column vector from a std vector of float. + */ vpColVector(const std::vector &v); #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + /*! + * Move constructor that take rvalue. + */ vpColVector(vpColVector &&v); vpColVector(const std::initializer_list &list) : vpArray2D(static_cast(list.size()), 1) { @@ -200,14 +255,14 @@ class VISP_EXPORT vpColVector : public vpArray2D } #endif /*! - Destructor. - */ + * Destructor. + */ virtual ~vpColVector() { } /*! - Removes all elements from the vector (which are destroyed), - leaving the container with a size of 0. - */ + * Removes all elements from the vector (which are destroyed), + * leaving the container with a size of 0. + */ void clear() { if (data != NULL) { @@ -222,14 +277,72 @@ class VISP_EXPORT vpColVector : public vpArray2D rowNum = colNum = dsize = 0; } + /*! + * Print to be used as part of a C++ code later. + * + * \param os : the stream to be printed in. + * \param matrixName : name of the column vector, "A" by default. + * \param octet : if false, print using double, if true, print byte per byte + * each bytes of the double array. + * + * The following code shows how to use this function: + * \code + * #include + * + * int main() + * { + * vpColVector v(3); + * for (unsigned int i=0; i + * + * int main() + * { + * std::ofstream ofs("log.csv", std::ofstream::out); + * vpColVector v(3); + * for (unsigned int i=0; i } /*! - Extract a sub-column vector from a column vector. - \param r : Index of the row corresponding to the first element of the - vector to extract. \param colsize : Size of the vector to extract. - \exception vpException::fatalError If the vector to extract is not - contained in the original one. - - \code - vpColVector v1; - for (unsigned int i=0; i<4; i++) - v1.stack(i); - // v1 is equal to [0 1 2 3]^T - vpColVector v2 = v1.extract(1, 3); - // v2 is equal to [1 2 3]^T - \endcode + * Extract a sub-column vector from a column vector. + * + * \param r : Index of the row corresponding to the first element of the vector to extract. + * \param colsize : Size of the vector to extract. + * \exception vpException::fatalError If the vector to extract is not + * contained in the original one. + * + * \code + * vpColVector v1; + * for (unsigned int i=0; i<4; i++) + * v1.stack(i); + * // v1 is equal to [0 1 2 3]^T + * vpColVector v2 = v1.extract(1, 3); + * // v2 is equal to [1 2 3]^T + * \endcode */ vpColVector extract(unsigned int r, unsigned int colsize) const { @@ -266,71 +380,631 @@ class VISP_EXPORT vpColVector : public vpArray2D return vpColVector(*this, r, colsize); } + /*! + * Compute and return the Frobenius norm \f$ ||v|| = \sqrt{ \sum_{v_{i}^2}} \f$ of + * all the elements \f$v_{i}\f$ of the column vector \f$ \bf v \f$ + * that is of dimension \f$ m \f$. + * + * \return The Frobenius norm if the vector is initialized, 0 otherwise. + * + * \sa infinityNorm() + * + */ double frobeniusNorm() const; + + /*! + * Compute the Hadamard product (element wise vector multiplication). + * + * \param v : Second vector; + * \return v1.hadamard(v2) The kronecker product : + * \f$ v1 \circ v2 = (v1 \circ v2)_{i} = (v1)_{i} (v2)_{i} \f$ + */ vpColVector hadamard(const vpColVector &v) const; + /*! + * Compute and return the infinity norm \f$ {||v||}_{\infty} = + * max\left({\mid v_{i} \mid}\right) \f$ with \f$i \in + * \{0, ..., m-1\}\f$ where \e m is the vector size and \f$v_i\f$ an element of + * the vector. + * + * \return The infinity norm if the matrix is initialized, 0 otherwise. + * + * \sa frobeniusNorm() + */ double infinityNorm() const; + + /*! + * Initialize the column vector from a part of an input column vector \e v. + * + * \param v : Input column vector used for initialization. + * \param r : row index in \e v that corresponds to the first element of the + * column vector to construct. + * \param nrows : Number of rows of the constructed + * column vector. + * + * The sub-vector starting from v[r] element and ending on v[r+nrows-1] element + * is used to initialize the constructed column vector. + * + * The following code shows how to use this function: + * \code + * #include + * + * int main() + * { + * vpColVector v(4); + * int val = 0; + * for(size_t i=0; i + * + * int main() + * { + * vpColVector v(4); + * for (unsigned int i=0; i < v.size(); i++) + * v[i] = i; + * std::cout << "v: " << v.t() << std::endl; + * + * vpColVector w(2); + * for (unsigned int i=0; i < w.size(); i++) + * w[i] = i+10; + * std::cout << "w: " << w.t() << std::endl; + * + * v.insert(1, w); + * std::cout << "v: " << v.t() << std::endl; + * } + * \endcode + * It produces the following output: + * \code + * v: 0 1 2 3 + * w: 10 11 + * v: 0 10 11 3 + * \endcode + */ void insert(unsigned int i, const vpColVector &v); + + /*! + * Insert a column vector. + * \param i : Index of the first element to introduce. This index starts from 0. + * \param v : Column vector to insert. + * + * The following example shows how to use this function: + * \code + * #include + * + * int main() + * { + * vpColVector v(4); + * for (unsigned int i=0; i < v.size(); i++) + * v[i] = i; + * std::cout << "v: " << v.t() << std::endl; + * + * vpColVector w(2); + * for (unsigned int i=0; i < w.size(); i++) + * w[i] = i+10; + * std::cout << "w: " << w.t() << std::endl; + * + * v.insert(w, 1); + * std::cout << "v: " << v.t() << std::endl; + * } + * \endcode + * It produces the following output: + * \code + * v: 0 1 2 3 + * w: 10 11 + * v: 0 10 11 3 + * \endcode + */ void insert(const vpColVector &v, unsigned int i); + /*! + * Print using Maple syntax, to copy/paste in Maple later. + * + * The following code + * \code + * #include + * + * int main() + * { + * vpColVector v(3); + * for (unsigned int i=0; i &list); #endif - //! Comparison operator. + + /*! + * Compare two column vectors. + * + * \param v : Vector to compare with. + * \return true when their respective size and their respective values are the same, + * false when their size or values differ. + */ bool operator==(const vpColVector &v) const; + + /*! + * Compare a column vector to a floating point value. + * + * \param v : Floating point value to compare with. + * \return true when all the values of the vector are equal to the floating point value `v`, + * false otherwise. + */ bool operator==(double v) const; + + /*! + * Compare two column vectors. + * + * \param v : Vector to compare with. + * \return true when their respective size or their values differ, false when their size and values are the same. + */ bool operator!=(const vpColVector &v) const; + + /*! + * Compare a column vector to a floating point value. + * + * \param v : Floating point value to compare with. + * \return true when at least one value of the vector differ from the floating point value `v`. + * false when all the vector values are equal to `v`. + */ bool operator!=(double v) const; - double operator*(const vpColVector &x) const; + /*! + * Operator that performs the dot product between two column vectors. + * + * \exception vpException::dimensionError If the vector dimension differ. + * + * \sa dotProd() + */ + double operator*(const vpColVector &v) const; + + /*! + * Multiply a column vector by a row vector. + * + * \param v : Row vector. + * + * \return The resulting matrix. + */ vpMatrix operator*(const vpRowVector &v) const; + + /*! + * Operator that allows to multiply each element of a column vector by a + * scalar. + * + * \param x : The scalar. + * + * \return The column vector multiplied by the scalar. The current + * column vector (*this) is unchanged. + * + * \code + * vpColVector v(3); + * v[0] = 1; + * v[1] = 2; + * v[2] = 3; + * + * vpColVector w = v * 3; + * // v is unchanged + * // w is now equal to : [3, 6, 9] + * \endcode + */ vpColVector operator*(double x) const; + + /*! + * Operator that allows to multiply each element of a column vector by a + * scalar. + * + * \param x : The scalar. + * + * \return The column vector multiplied by the scalar. + * + * \code + * vpColVector v(3); + * v[0] = 1; + * v[1] = 2; + * v[2] = 3; + * + * v *= 3; + * // v is now equal to : [3, 6, 9] + * \endcode + */ vpColVector &operator*=(double x); + /*! + * Operator that allows to divide each element of a column vector by a scalar. + * + * \param x : The scalar. + * + * \return The column vector divided by the scalar. The current + * column vector (*this) is unchanged. + * + * \code + * vpColVector v(3); + * v[0] = 8; + * v[1] = 4; + * v[2] = 2; + * + * vpColVector w = v / 2; + * // v is unchanged + * // w is now equal to : [4, 2, 1] + * \endcode + */ vpColVector operator/(double x) const; + + /*! + * Operator that allows to divide each element of a column vector by a scalar. + * + * \param x : The scalar. + * + * \return The column vector divided by the scalar. + * + * \code + * vpColVector v(3); + * v[0] = 8; + * v[1] = 4; + * v[2] = 2; + * + * v /= 2; + * // v is now equal to : [4, 2, 1] + * \endcode + */ vpColVector &operator/=(double x); + /*! + * Operator that allows to add two column vectors. + */ vpColVector operator+(const vpColVector &v) const; + + /*! + * Operator that allows to add a column vector to a translation vector. + * + * \param t : 3-dimension translation vector to add. + * + * \return The sum of the current column vector (*this) and the translation + * vector to add. + * \code + * vpTranslationVector t1(1,2,3); + * vpColVector v(3); + * v[0] = 4; + * v[1] = 5; + * v[2] = 6; + * vpTranslationVector t2; + * + * t2 = v + t1; + * // t1 and v leave unchanged + * // t2 is now equal to : 5, 7, 9 + * \endcode + */ vpTranslationVector operator+(const vpTranslationVector &t) const; + + /*! + * Operator that allows to add two column vectors. + */ vpColVector &operator+=(vpColVector v); + /*! + * Operator subtraction of two vectors this = this - v + */ vpColVector operator-(const vpColVector &v) const; + + /*! + * Operator that allows to subtract two column vectors. + */ vpColVector &operator-=(vpColVector v); + + /*! + * Operator that allows to negate all the column vector elements. + * + * \code + * vpColVector r(3, 1); + * // r contains [1 1 1]^T + * vpColVector v = -r; + * // v contains [-1 -1 -1]^T + * \endcode + */ vpColVector operator-() const; + /*! + * Copy operator. + * Allows operation such as A << v + * \code + * #include + * + * int main() + * { + * vpColVector A, B(5); + * for (unsigned int i=0; i + * + * int main() + * { + * size_t n = 5; + * vpColVector A(n); + * double *B = new double [n]; + * for (unsigned int i = 0; i < n; i++) + * B[i] = i; + * A << B; + * std::cout << "A: " << A.t() << std::endl; + * delete [] B; + * } + * \endcode + * It produces the following output: + * \code + * A: 0 1 2 3 4 + * \endcode + */ + vpColVector &operator<<(double *x); + + /*! + * This operator could be used to set column vector elements: + * \code + * #include &A) + */ int print(std::ostream &s, unsigned int length, char const *intro = 0) const; /*! - Converts a column vector containing angles in radians into degrees and returns a reference to the vector. - \return A reference to the vector with values expressed in [deg]. - \sa deg2rad() - */ + * Converts a column vector containing angles in radians into degrees and returns a reference + * to the vector. + * \return A reference to the vector with values expressed in [deg]. + * \sa deg2rad() + */ inline vpColVector &rad2deg() { double r2d = 180.0 / M_PI; @@ -339,26 +1013,100 @@ class VISP_EXPORT vpColVector : public vpArray2D return (*this); } + /*! + * Reshape the column vector in a matrix. + * + * \param M : the reshaped matrix. + * \param nrows : number of rows of the matrix. + * \param ncols : number of columns of the matrix. + * + * \exception vpException::dimensionError If the matrix and the column vector + * have not the same size. + * + * The following example shows how to use this method. + * \code + * #include + * + * int main() + * { + * int var=0; + * vpMatrix mat(3, 4); + * for (int i = 0; i < 3; i++) + * for (int j = 0; j < 4; j++) + * mat[i][j] = ++var; + * std::cout << "mat: \n" << mat << std::endl; + * + * vpColVector col = mat.stackColumns(); + * std::cout << "column vector: \n" << col << std::endl; + * + * vpMatrix remat = col.reshape(3, 4); + * std::cout << "remat: \n" << remat << std::endl; + * } + * \endcode + * + * If you run the previous example, you get: + * \code + * mat: + * 1 2 3 4 + * 5 6 7 8 + * 9 10 11 12 + * column vector: + * 1 + * 5 + * 9 + * 2 + * 6 + * 10 + * 3 + * 7 + * 11 + * 4 + * 8 + * 12 + * remat: + * 1 2 3 4 + * 5 6 7 8 + * 9 10 11 12 + * \endcode + */ void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols); - vpMatrix reshape(unsigned int nrows, unsigned int ncols); - /*! Modify the size of the column vector. - \param i : Size of the vector. This value corresponds to the vector number - of rows. - \param flagNullify : If true, set the data to zero. - \exception vpException::fatalError When \e ncols is not equal to 1. + /*! + * Reshape the column vector in a matrix. + * + * \param nrows : number of rows of the matrix + * \param ncols : number of columns of the matrix + * \return The reshaped matrix. + * + * \sa reshape(vpMatrix &, const unsigned int &, const unsigned int &) */ - void resize(unsigned int i, bool flagNullify = true) { vpArray2D::resize(i, 1, flagNullify); } + vpMatrix reshape(unsigned int nrows, unsigned int ncols); + /*! - Resize the column vector to a \e nrows-dimension vector. - This function can only be used with \e ncols = 1. - \param nrows : Vector number of rows. This value corresponds - to the size of the vector. - \param ncols : Vector number of columns. This value should be set to 1. - \param flagNullify : If true, set the data to zero. - \exception vpException::fatalError When \e ncols is not equal to 1. + * Modify the size of the column vector. + + * \param i : Size of the vector. This value corresponds to the vector number + * of rows. + * \param flagNullify : If true, set the data to zero. + * \exception vpException::fatalError When \e ncols is not equal to 1. + */ - */ + void resize(unsigned int i, bool flagNullify = true) + { + vpArray2D::resize(i, 1, flagNullify); + } + + /*! + * Resize the column vector to a \e nrows-dimension vector. + * This function can only be used with \e ncols = 1. + * + * \param nrows : Vector number of rows. This value corresponds + * to the size of the vector. + * \param ncols : Vector number of columns. This value should be set to 1. + * \param flagNullify : If true, set the data to zero. + * + * \exception vpException::fatalError When \e ncols is not equal to 1. + */ void resize(unsigned int nrows, unsigned int ncols, bool flagNullify) { if (ncols != 1) { @@ -370,44 +1118,267 @@ class VISP_EXPORT vpColVector : public vpArray2D vpArray2D::resize(nrows, ncols, flagNullify); } + /*! + * Stack column vector with a new element at the end of the vector. + * + * \param d : Element to stack to the existing vector. + * + * \code + * vpColVector v(3, 1); + * // v is equal to [1 1 1]^T + * v.stack(-2); + * // v is equal to [1 1 1 -2]^T + * \endcode + * + * \sa stack(const vpColVector &, const vpColVector &) + * \sa stack(const vpColVector &, const vpColVector &, vpColVector &) + */ void stack(double d); + + /*! + * Stack column vectors. + * + * \param v : Vector to stack to the existing one. + * + * \code + * vpColVector v1(3, 1); + * // v1 is equal to [1 1 1]^T + * vpColVector v2(2, 3); + * // v2 is equal to [3 3]^T + * v1.stack(v2); + * // v1 is equal to [1 1 1 3 3]^T + * \endcode + * + * \sa stack(const vpColVector &, const double &) + * \sa stack(const vpColVector &, const vpColVector &) + * \sa stack(const vpColVector &, const vpColVector &, vpColVector &) + */ void stack(const vpColVector &v); + /*! + * Return the sum of all the elements \f$v_{i}\f$ of the column vector \f$ \bf v \f$ + * that is of dimension \f$ m \f$. + * + * \return The value \f[ \sum_{i=0}^{m-1} v_i \f]. + */ double sum() const; + + /*! + * Return the sum of squares of all the elements \f$v_{i}\f$ of the column vector + * \f$ \bf v \f$ that is of dimension \f$ m \f$. + * + *\return The value \f[\sum_{i=0}^{m-1} v_i^{2}\f]. + */ double sumSquare() const; + + /*! + * Transpose the column vector. The resulting vector becomes a row vector. + */ vpRowVector t() const; + + /*! + * Converts the vpColVector to a std::vector. + * \return The corresponding std::vector. + */ std::vector toStdVector() const; + + /*! + * Transpose the column vector. The resulting vector becomes a row vector. + * \sa t() + */ vpRowVector transpose() const; - void transpose(vpRowVector &v) const; /*! - Compute and return the cross product of two 3-dimension vectors: \f$a - \times b\f$. \param a : 3-dimension column vector. \param b : 3-dimension - column vector. \return The cross product \f$a \times b\f$. + * Transpose the column vector. The resulting vector \e v becomes a row vector. + * \sa t() + */ + void transpose(vpRowVector &v) const; - \exception vpException::dimensionError If the vectors dimension is not - equal to 3. + /*! + * Compute and return the cross product of two 3-dimension vectors: \f$a + * \times b\f$. + * + * \param a : 3-dimension column vector. + * \param b : 3-dimension column vector. + * \return The cross product \f$a \times b\f$. + * + * \exception vpException::dimensionError If the vectors dimension is not + * equal to 3. + * + * \sa crossProd(), dotProd(), operator*(const vpColVector &) */ inline static vpColVector cross(const vpColVector &a, const vpColVector &b) { return crossProd(a, b); } + + /*! + * Compute and return the cross product of two vectors \f$a \times b\f$. + * + * \param[in] a : 3-dimension column vector. + * \param[in] b : 3-dimension column vector. + * \return The cross product \f$a \times b\f$. + * + * \exception vpException::dimensionError If the vectors dimension is not equal to 3. + * + * \sa dotProd() + */ static vpColVector crossProd(const vpColVector &a, const vpColVector &b); + /*! + * Compute end return the dot product of two column vectors: + * \f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of + * both vectors. + * + * \exception vpException::dimensionError If the vector dimension differ. + * + * \sa cross(), crossProd() + */ static double dotProd(const vpColVector &a, const vpColVector &b); + + /*! + * Return a column vector with elements of \e v that are reverse sorted with + * values going from greatest to lowest. + * + * Example: + * \code + * #include + * + * int main() + * { + * vpColVector v(10); + * v[0] = 5; v[1] = 7; v[2] = 4; v[3] = 2; v[4] = 8; + * v[5] = 6; v[6] = 1; v[7] = 9; v[8] = 0; v[9] = 3; + * + * std::cout << "v: " << v.t() << std::endl; + * + * vpColVector s = vpColVector::invSort(v); + * std::cout << "s: " << s.t() << std::endl; + * } + * \endcode + * Output: + * \code + * v: 5 7 4 2 8 6 1 9 0 3 + * s: 9 8 7 6 5 4 3 2 1 0 + * \endcode + * + * \sa sort() + */ static vpColVector invSort(const vpColVector &v); + + /*! + * Compute the median value of all the elements of the vector. + */ static double median(const vpColVector &v); + + /*! + * Compute the mean value of all the elements of the vector. + */ static double mean(const vpColVector &v); - // Compute the skew matrix [v]x + + /*! + * Compute the skew symmetric matrix \f$[{\bf v}]_\times\f$ of vector v. + * + * \f[ \mbox{if} \quad {\bf v} = \left( \begin{array}{c} x \\ y \\ z + * \end{array}\right), \quad \mbox{then} \qquad + * [{\bf v}]_\times = \left( \begin{array}{ccc} + * 0 & -z & y \\ + * z & 0 & -x \\ + * -y & x & 0 + * \end{array}\right) + * \f] + * + * \param v : Input vector used to compute the skew symmetric matrix. + */ static vpMatrix skew(const vpColVector &v); + /*! + * Return a column vector with elements of \e v that are sorted with values + * going from lowest to greatest. + * + * Example: + * \code + * #include + * + * int main() + * { + * vpColVector v(10); + * v[0] = 5; v[1] = 7; v[2] = 4; v[3] = 2; v[4] = 8; + * v[5] = 6; v[6] = 1; v[7] = 9; v[8] = 0; v[9] = 3; + * + * std::cout << "v: " << v.t() << std::endl; + * + * vpColVector s = vpColVector::sort(v); + * std::cout << "s: " << s.t() << std::endl; + * } + * \endcode + * Output: + * \code + * v: 5 7 4 2 8 6 1 9 0 3 + * s: 0 1 2 3 4 5 6 7 8 9 + * \endcode + * \sa invSort() + */ static vpColVector sort(const vpColVector &v); + /*! + * Stack column vectors. + * + * \param A : Initial vector. + * \param B : Vector to stack at the end of A. + * \return Stacked vector \f$[A B]^T\f$. + * + * \code + * vpColVector A(3); + * vpColVector B(5); + * vpColVector C; + * C = vpColVector::stack(A, B); // C = [A B]T + * // C is now an 8 dimension column vector + * \endcode + * + * \sa stack(const vpColVector &) + * \sa stack(const vpColVector &, const vpColVector &, vpColVector &) + */ static vpColVector stack(const vpColVector &A, const vpColVector &B); + + /*! + * Stack column vectors. + * + * \param A : Initial vector. + * \param B : Vector to stack at the end of A. + * \param C : Resulting stacked vector \f$C = [A B]^T\f$. + * + * \code{.cpp} + * vpColVector A(3); + * vpColVector B(5); + * vpColVector C; + * vpColVector::stack(A, B, C); // C = [A B]T + * // C is now an 8 dimension column vector + * \endcode + * + * \sa stack(const vpColVector &) + * \sa stack(const vpColVector &, const vpColVector &) + */ static void stack(const vpColVector &A, const vpColVector &B, vpColVector &C); + /*! + * Compute the standard deviation value of all the elements of the vector. + */ static double stdev(const vpColVector &v, bool useBesselCorrection = false); #ifdef VISP_HAVE_NLOHMANN_JSON - friend void to_json(nlohmann::json &j, const vpColVector &cam); - friend void from_json(const nlohmann::json &j, vpColVector &cam); + /*! + * Convert a vpColVector object to a JSON representation. + * + * @param j : Resulting json object. + * @param v : The object to convert. + */ + friend void to_json(nlohmann::json &j, const vpColVector &v); + + /*! + * Retrieve a vpColVector object from a JSON representation. + * + * @param j : JSON representation to convert. + * @param me : Converted object. + */ + friend void from_json(const nlohmann::json &j, vpColVector &v); #endif #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) @@ -416,45 +1387,75 @@ class VISP_EXPORT vpColVector : public vpArray2D */ //@{ /*! - \deprecated Provided only for compat with previous releases. - This function does nothing. + * \deprecated Provided only for compat with previous releases. + * This function does nothing. */ vp_deprecated void init() { } + /*! - \deprecated You should rather use extract(). + * \deprecated You should rather use extract(). */ vp_deprecated vpColVector rows(unsigned int first_row, unsigned int last_row) const { return vpColVector(*this, first_row - 1, last_row - first_row + 1); } + /*! - \deprecated You should rather use eye() + * \deprecated You should rather use eye() */ vp_deprecated void setIdentity(const double &val = 1.0); + /*! - \deprecated You should rather use stack(const vpColVector &) + * \deprecated You should rather use stack(const vpColVector &) */ vp_deprecated void stackMatrices(const vpColVector &r) { stack(r); } + /*! - \deprecated You should rather use stack(const vpColVector &A, const - vpColVector &B) + * \deprecated You should rather use stack(const vpColVector &A, const vpColVector &B) */ vp_deprecated static vpColVector stackMatrices(const vpColVector &A, const vpColVector &B) { return stack(A, B); } + /*! - \deprecated You should rather use stack(const vpColVector &A, const - vpColVector &B, vpColVector &C) + * \deprecated You should rather use stack(const vpColVector &A, const vpColVector &B, vpColVector &C) */ vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { stack(A, B, C); } + /*! + * \deprecated You should rather use insert(unsigned int, const vpColVector &). + * + * Insert column vector \e v at the given position \e r in the current column + * vector. + * + * \warning Throw vpMatrixException::incorrectMatrixSizeError if the + * dimensions of the matrices do not allow the operation. + * + * \param v : The column vector to insert. + * \param r : The index of the row to begin to insert data. + * \param c : Not used. + */ vp_deprecated void insert(const vpColVector &v, unsigned int r, unsigned int c = 0); + + /*! + * \deprecated This function is deprecated. You should rather use frobeniusNorm(). + * + * Compute and return the Euclidean norm also called Frobenius norm \f$ ||v|| = \sqrt{ \sum{v_{i}^2}} \f$. + * + * \return The Euclidean norm if the vector is initialized, 0 otherwise. + * + * \sa frobeniusNorm(), infinityNorm() + */ vp_deprecated double euclideanNorm() const; //@} #endif }; +/*! + * \relates vpColVector + * Allows to multiply a scalar by a column vector. + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS VISP_EXPORT #endif diff --git a/modules/core/include/visp3/core/vpException.h b/modules/core/include/visp3/core/vpException.h index 70b74a51bd..6d96c0fb62 100644 --- a/modules/core/include/visp3/core/vpException.h +++ b/modules/core/include/visp3/core/vpException.h @@ -77,7 +77,7 @@ class VISP_EXPORT vpException : public std::exception memoryFreeError, //!< Memory free error functionNotImplementedError, //!< Function not implemented ioError, //!< I/O error - cannotUseConstructorError, //!< Contructor error + cannotUseConstructorError, //!< constructor error notImplementedError, //!< Not implemented divideByZeroError, //!< Division by zero dimensionError, //!< Bad dimension diff --git a/modules/core/include/visp3/core/vpRxyzVector.h b/modules/core/include/visp3/core/vpRxyzVector.h index 330807fcec..da541877ff 100644 --- a/modules/core/include/visp3/core/vpRxyzVector.h +++ b/modules/core/include/visp3/core/vpRxyzVector.h @@ -134,7 +134,7 @@ class vpThetaUVector; \endcode The code below shows first how to initialize this representation of - Euler angles, than how to contruct a rotation matrix from a + Euler angles, than how to construct a rotation matrix from a vpRxyzVector and finally how to extract the vpRxyzVector Euler angles from the build rotation matrix. diff --git a/modules/core/include/visp3/core/vpRzyxVector.h b/modules/core/include/visp3/core/vpRzyxVector.h index fce560b5d1..0782f10c3e 100644 --- a/modules/core/include/visp3/core/vpRzyxVector.h +++ b/modules/core/include/visp3/core/vpRzyxVector.h @@ -137,7 +137,7 @@ class vpThetaUVector; \endcode The code below shows first how to initialize this representation of - Euler angles, than how to contruct a rotation matrix from a + Euler angles, than how to construct a rotation matrix from a vpRzyxVector and finally how to extract the vpRzyxVector Euler angles from the build rotation matrix. diff --git a/modules/core/include/visp3/core/vpRzyzVector.h b/modules/core/include/visp3/core/vpRzyzVector.h index fce5874764..897522f861 100644 --- a/modules/core/include/visp3/core/vpRzyzVector.h +++ b/modules/core/include/visp3/core/vpRzyzVector.h @@ -136,7 +136,7 @@ class vpThetaUVector; \endcode The code below shows first how to initialize this representation of - Euler angles, than how to contruct a rotation matrix from a + Euler angles, than how to construct a rotation matrix from a vpRzyzVector and finally how to extract the vpRzyzVector Euler angles from the build rotation matrix. diff --git a/modules/core/include/visp3/core/vpThetaUVector.h b/modules/core/include/visp3/core/vpThetaUVector.h index b1c6d10dc9..b940a6d4fa 100644 --- a/modules/core/include/visp3/core/vpThetaUVector.h +++ b/modules/core/include/visp3/core/vpThetaUVector.h @@ -126,7 +126,7 @@ class vpQuaternionVector; \endcode The code below shows first how to initialize a \f$\theta {\bf u}\f$ - vector, than how to contruct a rotation matrix from a vpThetaUVector + vector, than how to construct a rotation matrix from a vpThetaUVector and finally how to extract the theta U angles from the build rotation matrix. diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index ffddec970b..9c5a3d2bfe 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,14 +29,13 @@ * * Description: * Provide some simple operation on column vectors. - * -*****************************************************************************/ + */ /*! - \file vpColVector.cpp - \brief Class that provides a data structure for the column vectors as well - as a set of operations on these vectors -*/ + * \file vpColVector.cpp + * \brief Class that provides a data structure for the column vectors as well + * as a set of operations on these vectors + */ #include #include // std::fabs @@ -57,7 +55,6 @@ #include -//! Operator that allows to add two column vectors. vpColVector vpColVector::operator+(const vpColVector &v) const { if (getRows() != v.getRows()) { @@ -70,27 +67,7 @@ vpColVector vpColVector::operator+(const vpColVector &v) const r[i] = (*this)[i] + v[i]; return r; } -/*! - Operator that allows to add a column vector to a translation vector. - - \param t : 3-dimension translation vector to add. - - \return The sum of the current column vector (*this) and the translation - vector to add. -\code - vpTranslationVector t1(1,2,3); - vpColVector v(3); - v[0] = 4; - v[1] = 5; - v[2] = 6; - vpTranslationVector t2; - - t2 = v + t1; - // t1 and v leave unchanged - // t2 is now equal to : 5, 7, 9 - \endcode - -*/ + vpTranslationVector vpColVector::operator+(const vpTranslationVector &t) const { if (getRows() != 3) { @@ -105,7 +82,6 @@ vpTranslationVector vpColVector::operator+(const vpTranslationVector &t) const return s; } -//! Operator that allows to add two column vectors. vpColVector &vpColVector::operator+=(vpColVector v) { if (getRows() != v.getRows()) { @@ -117,7 +93,7 @@ vpColVector &vpColVector::operator+=(vpColVector v) (*this)[i] += v[i]; return (*this); } -//! Operator that allows to subtract two column vectors. + vpColVector &vpColVector::operator-=(vpColVector v) { if (getRows() != v.getRows()) { @@ -130,13 +106,6 @@ vpColVector &vpColVector::operator-=(vpColVector v) return (*this); } -/*! - Operator that performs the dot product between two column vectors. - - \exception vpException::dimensionError If the vector dimension differ. - - \sa dotProd() - */ double vpColVector::operator*(const vpColVector &v) const { if (size() != v.size()) { @@ -152,15 +121,6 @@ double vpColVector::operator*(const vpColVector &v) const return r; } -/*! - - Multiply a column vector by a row vector. - - \param v : Row vector. - - \return The resulting matrix. - -*/ vpMatrix vpColVector::operator*(const vpRowVector &v) const { vpMatrix M(rowNum, v.getCols()); @@ -172,7 +132,6 @@ vpMatrix vpColVector::operator*(const vpRowVector &v) const return M; } -//! operator subtraction of two vectors V = A-v vpColVector vpColVector::operator-(const vpColVector &m) const { if (getRows() != m.getRows()) { @@ -188,61 +147,11 @@ vpColVector vpColVector::operator-(const vpColVector &m) const return v; } -/*! - Construct a column vector from a part of an input column vector \e v. - - \param v : Input column vector used for initialization. - \param r : row index in \e v that corresponds to the first element of the - column vector to contruct. \param nrows : Number of rows of the constructed - column vector. - - The sub-vector starting from v[r] element and ending on v[r+nrows-1] element - is used to initialize the contructed column vector. - - \sa init() -*/ vpColVector::vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) : vpArray2D(nrows, 1) { init(v, r, nrows); } -/*! - Initialize the column vector from a part of an input column vector \e v. - - \param v : Input column vector used for initialization. - \param r : row index in \e v that corresponds to the first element of the - column vector to contruct. - \param nrows : Number of rows of the constructed - column vector. - - The sub-vector starting from v[r] element and ending on v[r+nrows-1] element - is used to initialize the contructed column vector. - - The following code shows how to use this function: -\code -#include - -int main() -{ - vpColVector v(4); - int val = 0; - for(size_t i=0; i(v.siz (*this)[i] = v[i]; } -//! Constructor that take column j of matrix M. vpColVector::vpColVector(const vpMatrix &M, unsigned int j) : vpArray2D(M.getRows(), 1) { for (unsigned int i = 0; i < M.getCols(); i++) (*this)[i] = M[i][j]; } -/*! - Constructor that creates a column vector from a m-by-1 matrix \e M. - - \exception vpException::dimensionError If the matrix is not a m-by-1 - matrix. - */ vpColVector::vpColVector(const vpMatrix &M) : vpArray2D(M.getRows(), 1) { if (M.getCols() != 1) { @@ -300,17 +202,12 @@ vpColVector::vpColVector(const vpMatrix &M) : vpArray2D(M.getRows(), 1) (*this)[i] = M[i][0]; } -/*! - Constructor that creates a column vector from a std vector of double. - */ vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsigned int)v.size(), 1) { for (unsigned int i = 0; i < v.size(); i++) (*this)[i] = v[i]; } -/*! - Constructor that creates a column vector from a std vector of float. - */ + vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsigned int)v.size(), 1) { for (unsigned int i = 0; i < v.size(); i++) @@ -318,9 +215,6 @@ vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsig } #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -/*! - Move constructor that take rvalue. - */ vpColVector::vpColVector(vpColVector &&v) : vpArray2D() { rowNum = v.rowNum; @@ -337,16 +231,6 @@ vpColVector::vpColVector(vpColVector &&v) : vpArray2D() } #endif -/*! - Operator that allows to negate all the column vector elements. - - \code - vpColVector r(3, 1); - // r contains [1 1 1]^T - vpColVector v = -r; - // v contains [-1 -1 -1]^T - \endcode - */ vpColVector vpColVector::operator-() const { vpColVector A; @@ -361,26 +245,6 @@ vpColVector vpColVector::operator-() const return A; } -/*! - Operator that allows to multiply each element of a column vector by a - scalar. - - \param x : The scalar. - - \return The column vector multiplied by the scalar. The current - column vector (*this) is unchanged. - - \code - vpColVector v(3); - v[0] = 1; - v[1] = 2; - v[2] = 3; - - vpColVector w = v * 3; - // v is unchanged - // w is now equal to : [3, 6, 9] - \endcode -*/ vpColVector vpColVector::operator*(double x) const { vpColVector v(rowNum); @@ -393,24 +257,6 @@ vpColVector vpColVector::operator*(double x) const return v; } -/*! - Operator that allows to multiply each element of a column vector by a - scalar. - - \param x : The scalar. - - \return The column vector multiplied by the scalar. - - \code - vpColVector v(3); - v[0] = 1; - v[1] = 2; - v[2] = 3; - - v *= 3; - // v is now equal to : [3, 6, 9] - \endcode -*/ vpColVector &vpColVector::operator*=(double x) { for (unsigned int i = 0; i < rowNum; i++) @@ -418,23 +264,6 @@ vpColVector &vpColVector::operator*=(double x) return (*this); } -/*! - Operator that allows to divide each element of a column vector by a scalar. - - \param x : The scalar. - - \return The column vector divided by the scalar. - - \code - vpColVector v(3); - v[0] = 8; - v[1] = 4; - v[2] = 2; - - v /= 2; - // v is now equal to : [4, 2, 1] - \endcode -*/ vpColVector &vpColVector::operator/=(double x) { for (unsigned int i = 0; i < rowNum; i++) @@ -442,25 +271,6 @@ vpColVector &vpColVector::operator/=(double x) return (*this); } -/*! - Operator that allows to divide each element of a column vector by a scalar. - - \param x : The scalar. - - \return The column vector divided by the scalar. The current - column vector (*this) is unchanged. - - \code - vpColVector v(3); - v[0] = 8; - v[1] = 4; - v[2] = 2; - - vpColVector w = v / 2; - // v is unchanged - // w is now equal to : [4, 2, 1] - \endcode -*/ vpColVector vpColVector::operator/(double x) const { vpColVector v(rowNum); @@ -473,11 +283,6 @@ vpColVector vpColVector::operator/(double x) const return v; } -/*! - Transform a m-by-1 matrix into a column vector. - \warning Handled with care; M should be a 1 column matrix. - \exception vpException::dimensionError If the matrix has more than 1 column. -*/ vpColVector &vpColVector::operator=(const vpMatrix &M) { if (M.getCols() != 1) { @@ -491,9 +296,6 @@ vpColVector &vpColVector::operator=(const vpMatrix &M) return (*this); } -/*! - Initialize a row vector from a standard vector of double. -*/ vpColVector &vpColVector::operator=(const std::vector &v) { resize((unsigned int)v.size(), false); @@ -501,9 +303,7 @@ vpColVector &vpColVector::operator=(const std::vector &v) (*this)[i] = v[i]; return *this; } -/*! - Initialize a row vector from a standard vector of double. -*/ + vpColVector &vpColVector::operator=(const std::vector &v) { resize((unsigned int)v.size(), false); @@ -523,9 +323,6 @@ vpColVector &vpColVector::operator=(const vpColVector &v) return *this; } -/*! - Operator that allows to convert a translation vector into a column vector. - */ vpColVector &vpColVector::operator=(const vpTranslationVector &tv) { unsigned int k = tv.getRows(); @@ -536,9 +333,7 @@ vpColVector &vpColVector::operator=(const vpTranslationVector &tv) memcpy(data, tv.data, rowNum * sizeof(double)); return *this; } -/*! - Operator that allows to convert a rotation vector into a column vector. - */ + vpColVector &vpColVector::operator=(const vpRotationVector &rv) { unsigned int k = rv.getRows(); @@ -549,9 +344,7 @@ vpColVector &vpColVector::operator=(const vpRotationVector &rv) memcpy(data, rv.data, rowNum * sizeof(double)); return *this; } -/*! - Operator that allows to convert a pose vector into a column vector. - */ + vpColVector &vpColVector::operator=(const vpPoseVector &p) { unsigned int k = p.getRows(); @@ -563,56 +356,12 @@ vpColVector &vpColVector::operator=(const vpPoseVector &p) return *this; } -/*! - Copy operator. - Allows operation such as A << v - \code -#include - -int main() -{ - vpColVector A, B(5); - for (unsigned int i=0; i - -int main() -{ - size_t n = 5; - vpColVector A(n); - double *B = new double [n]; - for (unsigned int i = 0; i < n; i++) - B[i] = i; - A << B; - std::cout << "A: " << A.t() << std::endl; - delete [] B; -} - \endcode - It produces the following output: - \code -A: 0 1 2 3 4 - \endcode - */ vpColVector &vpColVector::operator<<(double *x) { for (unsigned int i = 0; i < rowNum; i++) { @@ -623,24 +372,6 @@ vpColVector &vpColVector::operator<<(double *x) return *this; } -/*! - This operator could be used to set column vector elements: - \code -#include . - */ std::vector vpColVector::toStdVector() const { std::vector v(this->size()); @@ -697,9 +405,7 @@ std::vector vpColVector::toStdVector() const } #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -/*! - Overloaded move assignment operator taking rvalue. - */ + vpColVector &vpColVector::operator=(vpColVector &&other) { if (this != &other) { @@ -722,29 +428,6 @@ vpColVector &vpColVector::operator=(vpColVector &&other) return *this; } -/*! - Set vector elements and size from a list of values. - \param list : List of double. Vector size matches the number of elements. - \return The modified vector. - \code -#include - -int main() -{ - vpColVector c; - c = { 0, -1, -2 }; - std::cout << "c:\n" << c << std::endl; -} - \endcode - It produces the following printings: - \code -c: -0 --1 --2 - \endcode - \sa operator<<() - */ vpColVector &vpColVector::operator=(const std::initializer_list &list) { resize(static_cast(list.size()), false); @@ -753,12 +436,6 @@ vpColVector &vpColVector::operator=(const std::initializer_list &list) } #endif -/*! - * Compare two column vectors. - * \param v : Vector to compare with. - * \return true when their respective size and their respective values are the same, - * false when their size or values differ. - */ bool vpColVector::operator==(const vpColVector &v) const { if (rowNum != v.rowNum || colNum != v.colNum /* should not happen */) @@ -772,12 +449,6 @@ bool vpColVector::operator==(const vpColVector &v) const return true; } -/*! - * Compare a column vector to a floating point value. - * \param v : Floating point value to compare with. - * \return true when all the values of the vector are equal to the floating point value `v`, - * false otherwise. - */ bool vpColVector::operator==(double v) const { for (unsigned int i = 0; i < rowNum; i++) { @@ -788,24 +459,10 @@ bool vpColVector::operator==(double v) const return true; } -/*! - * Compare two column vectors. - * \param v : Vector to compare with. - * \return true when their respective size or their values differ, false when their size and values are the same. - */ bool vpColVector::operator!=(const vpColVector &v) const { return !(*this == v); } -/*! - * Compare a column vector to a floating point value. - * \param v : Floating point value to compare with. - * \return true when at least one value of the vector differ from the floating point value `v`. - * false when all the vector values are equal to `v`. - */ bool vpColVector::operator!=(double v) const { return !(*this == v); } -/*! - Transpose the column vector. The resulting vector becomes a row vector. -*/ vpRowVector vpColVector::t() const { vpRowVector v(rowNum); @@ -813,22 +470,10 @@ vpRowVector vpColVector::t() const return v; } -/*! - Transpose the column vector. The resulting vector becomes a row vector. - \sa t() -*/ vpRowVector vpColVector::transpose() const { return t(); } -/*! - Transpose the column vector. The resulting vector \e v becomes a row vector. - \sa t() -*/ void vpColVector::transpose(vpRowVector &v) const { v = t(); } -/*! - \relates vpColVector - Allows to multiply a scalar by a column vector. -*/ vpColVector operator*(const double &x, const vpColVector &v) { vpColVector vout; @@ -836,13 +481,6 @@ vpColVector operator*(const double &x, const vpColVector &v) return vout; } -/*! - Compute end return the dot product of two column vectors: - \f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of - both vectors. - - \exception vpException::dimensionError If the vector dimension differ. -*/ double vpColVector::dotProd(const vpColVector &a, const vpColVector &b) { if (a.data == NULL) { @@ -864,22 +502,11 @@ double vpColVector::dotProd(const vpColVector &a, const vpColVector &b) double c = 0; for (unsigned int i = 0; i < a.getRows(); i++) c += *(ad++) * *(bd++); - // vpMatrix c = (a.t() * b); - // return c[0][0]; + return c; } -/*! - Normalize a column vector. - - Considering the n-dim column vector \f$ {\bf x} = (x_0, x_1, \ldots, n_{n-1})\f$ normalize each vector element \f$ i - \f$: \f[ - x_i = \frac{x_i}{\sqrt{\sum_{i=0}^{n-1}x^2_i}} - \f] - \param[inout] x : As input, the vector to normalize, as output the normalized vector. - \return A reference to the normalized vector. -*/ vpColVector &vpColVector::normalize(vpColVector &x) const { x /= sqrt(x.sumSquare()); @@ -887,16 +514,6 @@ vpColVector &vpColVector::normalize(vpColVector &x) const return x; } -/*! - Normalize the column vector. - - Considering the n-dim column vector \f$ {\bf x} = (x_0, x_1, \ldots, n_{n-1})\f$ normalize each vector element \f$ i - \f$: \f[ - x_i = \frac{x_i}{\sqrt{\sum_{i=0}^{n-1}x^2_i}} - \f] - - \return A reference to the normalized vector. -*/ vpColVector &vpColVector::normalize() { @@ -910,34 +527,6 @@ vpColVector &vpColVector::normalize() return *this; } -/*! - Return a column vector with elements of \e v that are reverse sorted with - values going from greatest to lowest. - - Example: - \code -#include - -int main() -{ - vpColVector v(10); - v[0] = 5; v[1] = 7; v[2] = 4; v[3] = 2; v[4] = 8; - v[5] = 6; v[6] = 1; v[7] = 9; v[8] = 0; v[9] = 3; - - std::cout << "v: " << v.t() << std::endl; - - vpColVector s = vpColVector::invSort(v); - std::cout << "s: " << s.t() << std::endl; -} - \endcode - Output: - \code -v: 5 7 4 2 8 6 1 9 0 3 -s: 9 8 7 6 5 4 3 2 1 0 - \endcode - - \sa sort() - */ vpColVector vpColVector::invSort(const vpColVector &v) { if (v.data == NULL) { @@ -963,33 +552,6 @@ vpColVector vpColVector::invSort(const vpColVector &v) return tab; } -/*! - Return a column vector with elements of \e v that are sorted with values - going from lowest to geatest. - - Example: - \code -#include - -int main() -{ - vpColVector v(10); - v[0] = 5; v[1] = 7; v[2] = 4; v[3] = 2; v[4] = 8; - v[5] = 6; v[6] = 1; v[7] = 9; v[8] = 0; v[9] = 3; - - std::cout << "v: " << v.t() << std::endl; - - vpColVector s = vpColVector::sort(v); - std::cout << "s: " << s.t() << std::endl; -} - \endcode - Output: - \code -v: 5 7 4 2 8 6 1 9 0 3 -s: 0 1 2 3 4 5 6 7 8 9 - \endcode - \sa invSort() - */ vpColVector vpColVector::sort(const vpColVector &v) { if (v.data == NULL) { @@ -1015,67 +577,14 @@ vpColVector vpColVector::sort(const vpColVector &v) return tab; } -/*! - Stack column vector with a new element at the end of the vector. - - \param d : Element to stack to the existing vector. - - \code - vpColVector v(3, 1); - // v is equal to [1 1 1]^T - v.stack(-2); - // v is equal to [1 1 1 -2]^T - \endcode - - \sa stack(const vpColVector &, const vpColVector &) - \sa stack(const vpColVector &, const vpColVector &, vpColVector &) - -*/ void vpColVector::stack(double d) { this->resize(rowNum + 1, false); (*this)[rowNum - 1] = d; } -/*! - Stack column vectors. - - \param v : Vector to stack to the existing one. - - \code - vpColVector v1(3, 1); - // v1 is equal to [1 1 1]^T - vpColVector v2(2, 3); - // v2 is equal to [3 3]^T - v1.stack(v2); - // v1 is equal to [1 1 1 3 3]^T - \endcode - - \sa stack(const vpColVector &, const double &) - \sa stack(const vpColVector &, const vpColVector &) - \sa stack(const vpColVector &, const vpColVector &, vpColVector &) - -*/ void vpColVector::stack(const vpColVector &v) { *this = vpColVector::stack(*this, v); } -/*! - Stack column vectors. - - \param A : Initial vector. - \param B : Vector to stack at the end of A. - \return Stacked vector \f$[A B]^T\f$. - - \code - vpColVector A(3); - vpColVector B(5); - vpColVector C; - C = vpColVector::stack(A, B); // C = [A B]T - // C is now an 8 dimension column vector - \endcode - - \sa stack(const vpColVector &) - \sa stack(const vpColVector &, const vpColVector &, vpColVector &) -*/ vpColVector vpColVector::stack(const vpColVector &A, const vpColVector &B) { vpColVector C; @@ -1083,24 +592,6 @@ vpColVector vpColVector::stack(const vpColVector &A, const vpColVector &B) return C; } -/*! - Stack column vectors. - - \param A : Initial vector. - \param B : Vector to stack at the end of A. - \param C : Resulting stacked vector \f$C = [A B]^T\f$. - - \code - vpColVector A(3); - vpColVector B(5); - vpColVector C; - vpColVector::stack(A, B, C); // C = [A B]T - // C is now an 8 dimension column vector - \endcode - - \sa stack(const vpColVector &) - \sa stack(const vpColVector &, const vpColVector &) -*/ void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C) { unsigned int nrA = A.getRows(); @@ -1131,9 +622,6 @@ void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector C[nrA + i] = B[i]; } -/*! - Compute the mean value of all the elements of the vector. -*/ double vpColVector::mean(const vpColVector &v) { if (v.data == NULL || v.size() == 0) { @@ -1143,17 +631,9 @@ double vpColVector::mean(const vpColVector &v) // Use directly sum() function double mean = v.sum(); - // Old code used - // double *vd = v.data; - // for (unsigned int i=0 ; i < v.getRows() ; i++) - // mean += *(vd++); - return mean / v.getRows(); } -/*! - Compute the median value of all the elements of the vector. -*/ double vpColVector::median(const vpColVector &v) { if (v.data == NULL || v.size() == 0) { @@ -1165,9 +645,6 @@ double vpColVector::median(const vpColVector &v) return vpMath::getMedian(vectorOfDoubles); } -/*! - Compute the standard deviation value of all the elements of the vector. -*/ double vpColVector::stdev(const vpColVector &v, bool useBesselCorrection) { if (v.data == NULL || v.size() == 0) { @@ -1177,20 +654,6 @@ double vpColVector::stdev(const vpColVector &v, bool useBesselCorrection) return SimdVectorStdev(v.data, v.rowNum, useBesselCorrection); } -/*! - Compute the skew symmetric matrix \f$[{\bf v}]_\times\f$ of vector v. - - \f[ \mbox{if} \quad {\bf V} = \left( \begin{array}{c} x \\ y \\ z - \end{array}\right), \quad \mbox{then} \qquad - [{\bf v}]_\times = \left( \begin{array}{ccc} - 0 & -z & y \\ - z & 0 & -x \\ - -y & x & 0 - \end{array}\right) - \f] - - \param v : Input vector used to compute the skew symmetric matrix. -*/ vpMatrix vpColVector::skew(const vpColVector &v) { vpMatrix M; @@ -1213,15 +676,6 @@ vpMatrix vpColVector::skew(const vpColVector &v) return M; } -/*! - Compute and return the cross product of two vectors \f$a \times b\f$. - - \param[in] a : 3-dimension column vector. - \param[in] b : 3-dimension column vector. - \return The cross product \f$a \times b\f$. - - \exception vpException::dimensionError If the vectors dimension is not equal to 3. -*/ vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b) { if (a.getRows() != 3 || b.getRows() != 3) { @@ -1234,14 +688,6 @@ vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b) return vpColVector::skew(a) * b; } -/*! - Reshape the column vector in a matrix. - \param nrows : number of rows of the matrix - \param ncols : number of columns of the matrix - \return The reshaped matrix. - - \sa reshape(vpMatrix &, const unsigned int &, const unsigned int &) -*/ vpMatrix vpColVector::reshape(unsigned int nrows, unsigned int ncols) { vpMatrix M(nrows, ncols); @@ -1249,61 +695,6 @@ vpMatrix vpColVector::reshape(unsigned int nrows, unsigned int ncols) return M; } -/*! - Reshape the column vector in a matrix. - \param M : the reshaped matrix. - \param nrows : number of rows of the matrix. - \param ncols : number of columns of the matrix. - - \exception vpException::dimensionError If the matrix and the column vector -have not the same size. - - The following example shows how to use this method. - \code -#include - -int main() -{ - int var=0; - vpMatrix mat(3, 4); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 4; j++) - mat[i][j] = ++var; - std::cout << "mat: \n" << mat << std::endl; - - vpColVector col = mat.stackColumns(); - std::cout << "column vector: \n" << col << std::endl; - - vpMatrix remat = col.reshape(3, 4); - std::cout << "remat: \n" << remat << std::endl; -} - \endcode - - If you run the previous example, you get: - \code -mat: -1 2 3 4 -5 6 7 8 -9 10 11 12 -column vector: -1 -5 -9 -2 -6 -10 -3 -7 -11 -4 -8 -12 -remat: -1 2 3 4 -5 6 7 8 -9 10 11 12 - \endcode -*/ void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols) { if (dsize != nrows * ncols) { @@ -1318,38 +709,6 @@ void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned M[i][j] = data[j * nrows + i]; } -/*! - Insert a column vector. - \param i : Index of the first element to introduce. This index starts from -0. \param v : Column vector to insert. - - The following example shows how to use this function: - \code -#include - -int main() -{ - vpColVector v(4); - for (unsigned int i=0; i < v.size(); i++) - v[i] = i; - std::cout << "v: " << v.t() << std::endl; - - vpColVector w(2); - for (unsigned int i=0; i < w.size(); i++) - w[i] = i+10; - std::cout << "w: " << w.t() << std::endl; - - v.insert(1, w); - std::cout << "v: " << v.t() << std::endl; -} - \endcode - It produces the following output: - \code -v: 0 1 2 3 -w: 10 11 -v: 0 10 11 3 - \endcode - */ void vpColVector::insert(unsigned int i, const vpColVector &v) { if (i + v.size() > this->size()) @@ -1361,33 +720,9 @@ void vpColVector::insert(unsigned int i, const vpColVector &v) } void vpColVector::insert(const vpColVector &v, unsigned int i) { - if (i + v.size() > this->size()) - throw(vpException(vpException::dimensionError, "Unable to insert a column vector")); - - if (data != NULL && v.data != NULL && v.rowNum > 0) { - memcpy(data + i, v.data, sizeof(double) * v.rowNum); - } + insert(i, v); } -/*! - - Pretty print a column vector. The data are tabulated. - The common widths before and after the decimal point - are set with respect to the parameter maxlen. - - \param s Stream used for the printing. - - \param length The suggested width of each vector element. - The actual width grows in order to accomodate the whole integral part, - and shrinks if the whole extent is not needed for all the numbers. - \param intro The introduction which is printed before the vector. - Can be set to zero (or omitted), in which case - the introduction is not printed. - - \return Returns the common total width for all vector elements. - - \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) -*/ int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; @@ -1474,31 +809,10 @@ int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) return (int)(maxBefore + maxAfter); } -/*! - Return the sum of all the elements \f$v_{i}\f$ of the column vector v(m). - - \return The value \f[\sum{i=0}^{m} v_i\f]. - */ double vpColVector::sum() const { return SimdVectorSum(data, rowNum); } -/*! - Return the sum square of all the elements \f$v_{i}\f$ of the column vector - v(m). - - \return The value \f[\sum{i=0}^{m} v_i^{2}\f]. - */ double vpColVector::sumSquare() const { return SimdVectorSumSquare(data, rowNum); } - - -/*! - Compute and return the Frobenius norm \f$ ||v|| = \sqrt{ \sum{v_{i}^2}} \f$. - - \return The Frobenius norm if the vector is initialized, 0 otherwise. - - \sa infinityNorm() - -*/ double vpColVector::frobeniusNorm() const { double norm = sumSquare(); @@ -1506,12 +820,6 @@ double vpColVector::frobeniusNorm() const return sqrt(norm); } -/*! - Compute the Hadamard product (element wise vector multiplication). - \param v : Second vector; - \return v1.hadamard(v2) The kronecker product : - \f$ v1 \circ v2 = (v1 \circ v2)_{i} = (v1)_{i} (v2)_{i} \f$ -*/ vpColVector vpColVector::hadamard(const vpColVector &v) const { if (v.getRows() != rowNum || v.getCols() != colNum) { @@ -1526,17 +834,6 @@ vpColVector vpColVector::hadamard(const vpColVector &v) const return out; } -/*! - - Compute and return the infinity norm \f$ {||v||}_{\infty} = - max\left({\mid v_{i} \mid}\right) \f$ with \f$i \in - \{0, ..., m-1\}\f$ where \e m is the vector size and \f$v_i\f$ an element of - the vector. - - \return The infinity norm if the matrix is initialized, 0 otherwise. - - \sa frobeniusNorm() -*/ double vpColVector::infinityNorm() const { double norm = 0.0; @@ -1549,34 +846,6 @@ double vpColVector::infinityNorm() const return norm; } -/*! - Print to be used as part of a C++ code later. - - \param os : the stream to be printed in. - \param matrixName : name of the column vector, "A" by default. - \param octet : if false, print using double, if true, print byte per byte - each bytes of the double array. - - The following code shows how to use this function: -\code -#include - -int main() -{ - vpColVector v(3); - for (unsigned int i=0; igetRows() << "); " << std::endl; @@ -1597,32 +866,6 @@ std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixN return os; }; -/*! - Print/save a column vector in csv format. - - The following code - \code -#include - -int main() -{ - std::ofstream ofs("log.csv", std::ofstream::out); - vpColVector v(3); - for (unsigned int i=0; igetRows(); ++i) { @@ -1633,31 +876,6 @@ std::ostream &vpColVector::csvPrint(std::ostream &os) const return os; }; -/*! - Print using Maple syntax, to copy/paste in Maple later. - - The following code - \code -#include - -int main() -{ - vpColVector v(3); - for (unsigned int i=0; i - -int main() -{ - vpColVector v(3); - for (unsigned int i=0; i &v) : vpArray2D(1, (un \param v : Input row vector used for initialization. \param c : column index in \e v that corresponds to the first element of the - row vector to contruct. \param ncols : Number of columns of the constructed + row vector to construct. \param ncols : Number of columns of the constructed row vector. The sub-vector starting from v[c] element and ending on v[c+ncols-1] element - is used to initialize the contructed row vector. + is used to initialize the constructed row vector. \sa init() */ @@ -947,7 +947,7 @@ double vpRowVector::stdev(const vpRowVector &v, bool useBesselCorrection) \param s Stream used for the printing. \param length The suggested width of each row vector element. - The actual width grows in order to accomodate the whole integral part, + The actual width grows in order to accommodate the whole integral part, and shrinks if the whole extent is not needed for all the numbers. \param intro The introduction which is printed before the vector. Can be set to zero (or omitted), in which case @@ -1116,11 +1116,11 @@ double vpRowVector::euclideanNorm() const { return frobeniusNorm(); } \param v : Input row vector used for initialization. \param c : column index in \e v that corresponds to the first element of the - row vector to contruct. \param ncols : Number of columns of the constructed + row vector to construct. \param ncols : Number of columns of the constructed row vector. The sub-vector starting from v[c] element and ending on v[c+ncols-1] element - is used to initialize the contructed row vector. + is used to initialize the constructed row vector. The following code shows how to use this function: \code diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp index dd9553241f..826f1a9c2c 100644 --- a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp @@ -532,7 +532,7 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, b \param s Stream used for the printing. \param length The suggested width of each matrix element. - The actual width grows in order to accomodate the whole integral part, + The actual width grows in order to accommodate the whole integral part, and shrinks if the whole extent is not needed for all the numbers. \param intro The introduction which is printed before the matrix. Can be set to zero (or omitted), in which case diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp index c1489f32b9..3a52613103 100644 --- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp @@ -935,7 +935,7 @@ void vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const { M = inverse(); The code below shows how to save an homogeneous matrix in a file. \code - // Contruct an homogeneous matrix + // construct an homogeneous matrix vpTranslationVector t(1,2,3); vpRxyzVector r(M_PI, 0, -M_PI/4.); vpRotationMatrix R(r); diff --git a/modules/core/src/math/transformation/vpPoseVector.cpp b/modules/core/src/math/transformation/vpPoseVector.cpp index 7fef5c61f8..44e821c27c 100644 --- a/modules/core/src/math/transformation/vpPoseVector.cpp +++ b/modules/core/src/math/transformation/vpPoseVector.cpp @@ -404,7 +404,7 @@ vpRowVector vpPoseVector::t() const \param s Stream used for the printing. \param length The suggested width of each vector element. - The actual width grows in order to accomodate the whole integral part, + The actual width grows in order to accommodate the whole integral part, and shrinks if the whole extent is not needed for all the numbers. \param intro The introduction which is printed before the vector. Can be set to zero (or omitted), in which case diff --git a/modules/core/src/math/transformation/vpRotationMatrix.cpp b/modules/core/src/math/transformation/vpRotationMatrix.cpp index 7f8b39307a..074dd1bb8f 100644 --- a/modules/core/src/math/transformation/vpRotationMatrix.cpp +++ b/modules/core/src/math/transformation/vpRotationMatrix.cpp @@ -478,7 +478,7 @@ bool vpRotationMatrix::isARotationMatrix(double threshold) const vpRotationMatrix::vpRotationMatrix() : vpArray2D(3, 3), m_index(0) { eye(); } /*! - Copy contructor that construct a 3-by-3 rotation matrix from another + Copy constructor that construct a 3-by-3 rotation matrix from another rotation matrix. */ vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D(3, 3), m_index(0) { (*this) = M; } diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp index 94373b6fef..43cedfcc8a 100644 --- a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp @@ -492,7 +492,7 @@ void vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const \param s Stream used for the printing. \param length The suggested width of each matrix element. - The actual width grows in order to accomodate the whole integral part, + The actual width grows in order to accommodate the whole integral part, and shrinks if the whole extent is not needed for all the numbers. \param intro The introduction which is printed before the matrix. Can be set to zero (or omitted), in which case diff --git a/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h b/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h index e0010b5367..09c8ef7603 100644 --- a/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h +++ b/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h @@ -615,4 +615,4 @@ vpDetectorDNNOpenCV::DetectedFeatures2D::display(const vpImage< Type > &img, con vpDisplay::displayText(img, m_bbox.getTopRight(), ss.str(), color); } #endif -#endif \ No newline at end of file +#endif diff --git a/modules/gui/src/pointcloud/vpPclViewer.cpp b/modules/gui/src/pointcloud/vpPclViewer.cpp index b7c5b9c1da..a8ed45b2cb 100755 --- a/modules/gui/src/pointcloud/vpPclViewer.cpp +++ b/modules/gui/src/pointcloud/vpPclViewer.cpp @@ -491,4 +491,4 @@ void vpPclViewer::threadUpdateSurfaceOriginalColor(const pclPointCloudPointXYZRG // symbols void dummy_vpPCLPointCLoudVisualization() { }; #endif -#endif \ No newline at end of file +#endif diff --git a/modules/io/include/visp3/io/vpImageQueue.h b/modules/io/include/visp3/io/vpImageQueue.h index b300d54701..97ca1c4c91 100644 --- a/modules/io/include/visp3/io/vpImageQueue.h +++ b/modules/io/include/visp3/io/vpImageQueue.h @@ -61,19 +61,20 @@ template class vpImageQueue { public: - struct cancelled { - }; + struct vpCancelled_t + { }; /*! * Queue (FIFO) constructor. By default the max queue size is set to 1024*8. * * \param[in] seqname : Generic sequence name like `"folder/I%04d.png"`. If this name contains a parent folder, it - * will be created. \param[in] record_mode : 0 to record a sequence of images, 1 to record single images. + * will be created. + * \param[in] record_mode : 0 to record a sequence of images, 1 to record single images. */ vpImageQueue(const std::string &seqname, int record_mode) : m_cancelled(false), m_cond(), m_queue_image(), m_queue_data(), m_maxQueueSize(1024 * 8), m_mutex(), - m_seqname(seqname), m_recording_mode(record_mode), m_start_recording(false), m_directory_to_create(false), - m_recording_trigger(false) + m_seqname(seqname), m_recording_mode(record_mode), m_start_recording(false), m_directory_to_create(false), + m_recording_trigger(false) { m_directory = vpIoTools::getParent(seqname); if (!m_directory.empty()) { @@ -82,7 +83,7 @@ template class vpImageQueue } } m_text_record_mode = - std::string("Record mode: ") + (m_recording_mode ? std::string("single") : std::string("continuous")); + std::string("Record mode: ") + (m_recording_mode ? std::string("single") : std::string("continuous")); } /*! @@ -124,13 +125,13 @@ template class vpImageQueue while (m_queue_image.empty()) { if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } m_cond.wait(lock); if (m_cancelled) { - throw cancelled(); + throw vpCancelled_t(); } } @@ -194,19 +195,22 @@ template class vpImageQueue vpDisplay::displayText(I, 20 * vpDisplay::getDownScalingFactor(I), 10 * vpDisplay::getDownScalingFactor(I), "Left click: stop recording", vpColor::red); - } else { + } + else { vpDisplay::displayText(I, 20 * vpDisplay::getDownScalingFactor(I), 10 * vpDisplay::getDownScalingFactor(I), "Left click: start recording", vpColor::red); } - } else { + } + else { vpDisplay::displayText(I, 20 * vpDisplay::getDownScalingFactor(I), 10 * vpDisplay::getDownScalingFactor(I), "Left click: record image", vpColor::red); } } vpDisplay::displayText(I, 40 * vpDisplay::getDownScalingFactor(I), 10 * vpDisplay::getDownScalingFactor(I), "Right click: quit", vpColor::red); - } else { + } + else { vpDisplay::displayText(I, 20 * vpDisplay::getDownScalingFactor(I), 10 * vpDisplay::getDownScalingFactor(I), "Click to quit", vpColor::red); } @@ -220,14 +224,17 @@ template class vpImageQueue if (!m_seqname.empty()) { // Recording requested if (button == vpMouseButton::button1 && !disable_left_click) { // enable/disable recording m_start_recording = !m_start_recording; - } else if (button == vpMouseButton::button3) { // quit + } + else if (button == vpMouseButton::button3) { // quit return true; } - } else { // any button to quit + } + else { // any button to quit return true; } } - } else if (!m_seqname.empty()) { + } + else if (!m_seqname.empty()) { m_start_recording = true; } diff --git a/modules/io/include/visp3/io/vpImageStorageWorker.h b/modules/io/include/visp3/io/vpImageStorageWorker.h index 2fefad8c32..9fc16fab51 100644 --- a/modules/io/include/visp3/io/vpImageStorageWorker.h +++ b/modules/io/include/visp3/io/vpImageStorageWorker.h @@ -81,7 +81,8 @@ template class vpImageStorageWorker if (m_record_mode > 0) { // Single image std::cout << "Save image: " << filename << std::endl; - } else if (m_cpt == 1) { + } + else if (m_cpt == 1) { std::cout << "Started sequence saving: " << m_seqname << std::endl; } vpImageIo::write(I, filename); @@ -105,13 +106,15 @@ template class vpImageStorageWorker m_cpt++; } - } catch (const vpImageQueue::cancelled &) { + } + catch (const vpImageQueue::vpCancelled_t &) { std::cout << "Receive cancel during color image saving." << std::endl; if (m_data_file_created) { std::cout << "Close data file: " << m_dataname << std::endl; m_ofs_data.close(); } - } catch (const vpImageQueue::cancelled &) { + } + catch (const vpImageQueue::vpCancelled_t &) { std::cout << "Receive cancel during gray image saving." << std::endl; if (m_data_file_created) { std::cout << "Close data file: " << m_dataname << std::endl; diff --git a/modules/io/include/visp3/io/vpJsonArgumentParser.h b/modules/io/include/visp3/io/vpJsonArgumentParser.h index 0ce17b6536..4cee71ee34 100644 --- a/modules/io/include/visp3/io/vpJsonArgumentParser.h +++ b/modules/io/include/visp3/io/vpJsonArgumentParser.h @@ -177,7 +177,7 @@ class VISP_EXPORT vpJsonArgumentParser template vpJsonArgumentParser &addArgument(const std::string &name, T ¶meter, const bool required = true, const std::string &help = "No description") { - const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json * { + const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json *{ size_t pos = 0; nlohmann::json *f = &j; std::string token; @@ -203,7 +203,7 @@ class VISP_EXPORT vpJsonArgumentParser } f = &(f->at(name_copy)); return f; - }; + }; parsers[name] = [¶meter, required, getter, name](nlohmann::json &j) { const nlohmann::json *field = getter(j, false); @@ -216,12 +216,12 @@ class VISP_EXPORT vpJsonArgumentParser else if (!fieldHasNoValue) { field->get_to(parameter); } - }; + }; updaters[name] = [getter](nlohmann::json &j, const std::string &s) { nlohmann::json *field = getter(j, true); *field = convertCommandLineArgument(s); - }; + }; helpers[name] = [help, parameter, required]() -> std::string { std::stringstream ss; @@ -234,7 +234,7 @@ class VISP_EXPORT vpJsonArgumentParser ss << std::endl << "Optional"; } return ss.str(); - }; + }; nlohmann::json *exampleField = getter(exampleJson, true); *exampleField = parameter; @@ -248,7 +248,7 @@ class VISP_EXPORT vpJsonArgumentParser * @param argc Number of arguments (including program name) * @param argv Arguments */ - void parse(int argc, const char *argv []); + void parse(int argc, const char *argv[]); private: @@ -263,4 +263,4 @@ class VISP_EXPORT vpJsonArgumentParser #endif // VISP_HAVE_NLOHMANN_JSON -#endif \ No newline at end of file +#endif diff --git a/modules/io/src/tools/vpJsonArgumentParser.cpp b/modules/io/src/tools/vpJsonArgumentParser.cpp index b392eca1f7..9c605fd767 100644 --- a/modules/io/src/tools/vpJsonArgumentParser.cpp +++ b/modules/io/src/tools/vpJsonArgumentParser.cpp @@ -37,7 +37,6 @@ using json = nlohmann::json; //! json namespace shortcut - vpJsonArgumentParser::vpJsonArgumentParser(const std::string &description, const std::string &jsonFileArgumentName, const std::string &nestSeparator) : description(description), @@ -53,8 +52,8 @@ vpJsonArgumentParser::vpJsonArgumentParser(const std::string &description, const } helpers[jsonFileArgumentName] = []() -> std::string { - return "Path to the JSON configuration file. Values in this files are loaded, and can be overriden by command line arguments.\nOptional"; - }; + return "Path to the JSON configuration file. Values in this files are loaded, and can be overridden by command line arguments.\nOptional"; + }; } std::string vpJsonArgumentParser::help() const @@ -94,8 +93,7 @@ std::string vpJsonArgumentParser::help() const return ss.str(); } - -void vpJsonArgumentParser::parse(int argc, const char *argv []) +void vpJsonArgumentParser::parse(int argc, const char *argv[]) { json j; const std::vector arguments(argv + 1, argv + argc); @@ -152,4 +150,4 @@ void vpJsonArgumentParser::parse(int argc, const char *argv []) } } -#endif \ No newline at end of file +#endif diff --git a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp index 44d55bfaa4..9497e53e5a 100644 --- a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp +++ b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp @@ -120,7 +120,7 @@ void emergencyStopAfma4(int signo) The only available constructor. - This contructor calls init() to initialise the connection with the MotionBox + This constructor calls init() to initialise the connection with the MotionBox or low level controller, power on the robot and wait 1 sec before returning to be sure the initialisation is done. diff --git a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp index 65c41dd472..61d8985d35 100644 --- a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp @@ -121,7 +121,7 @@ void emergencyStopAfma6(int signo) The only available constructor. - This contructor calls init() to initialise + This constructor calls init() to initialise the connection with the MotionBox or low level controller, send the default eMc homogeneous matrix, power on the robot and wait 1 sec before returning to be sure the initialisation is done. diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index 5a1798ce6d..8273e35e77 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -61,7 +61,7 @@ using namespace std::chrono_literals; class vpRobotMavsdk::vpRobotMavsdkImpl { public: - vpRobotMavsdkImpl() : m_takeoffAlt(1.0) {} + vpRobotMavsdkImpl() : m_takeoffAlt(1.0) { } vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); } virtual ~vpRobotMavsdkImpl() @@ -80,7 +80,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) { std::cout << "Waiting to discover system..." << std::endl; - auto prom = std::promise >{}; + auto prom = std::promise > {}; auto fut = prom.get_future(); // We wait for new systems to be discovered, once we find one that has an @@ -118,9 +118,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl MAV_TYPE getVehicleType() { - auto passthrough = mavsdk::MavlinkPassthrough{m_system}; + auto passthrough = mavsdk::MavlinkPassthrough { m_system }; - auto prom = std::promise{}; + auto prom = std::promise {}; auto fut = prom.get_future(); #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) mavsdk::MavlinkPassthrough::MessageHandle handle = passthrough.subscribe_message( @@ -130,21 +130,21 @@ class vpRobotMavsdk::vpRobotMavsdkImpl [&passthrough, &prom](const mavlink_message_t &message) { #endif // Process only Heartbeat coming from the autopilot - if (message.compid != MAV_COMP_ID_AUTOPILOT1) { - return; - } + if (message.compid != MAV_COMP_ID_AUTOPILOT1) { + return; + } - mavlink_heartbeat_t heartbeat; - mavlink_msg_heartbeat_decode(&message, &heartbeat); + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(&message, &heartbeat); - // Unsubscribe again as we only want to find one system. + // Unsubscribe again as we only want to find one system. #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) - passthrough.unsubscribe_message(handle); + passthrough.unsubscribe_message(handle); #else passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); #endif - prom.set_value(static_cast(heartbeat.type)); + prom.set_value(static_cast(heartbeat.type)); }); // We usually receive heartbeats at 1Hz, therefore we should find a @@ -171,29 +171,29 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } std::function - create_calibration_callback(std::promise &calibration_promise) + create_calibration_callback(std::promise &calibration_promise) { return [&calibration_promise](const mavsdk::Calibration::Result result, const mavsdk::Calibration::ProgressData progress_data) { - switch (result) { - case mavsdk::Calibration::Result::Success: - std::cout << "--- Calibration succeeded!" << std::endl; - calibration_promise.set_value(); - break; - case mavsdk::Calibration::Result::Next: - if (progress_data.has_progress) { - std::cout << " Progress: " << progress_data.progress << std::endl; - } - if (progress_data.has_status_text) { - std::cout << " Instruction: " << progress_data.status_text << std::endl; - } - break; - default: - std::cout << "--- Calibration failed with message: " << result << std::endl; - calibration_promise.set_value(); - break; - } - }; + switch (result) { + case mavsdk::Calibration::Result::Success: + std::cout << "--- Calibration succeeded!" << std::endl; + calibration_promise.set_value(); + break; + case mavsdk::Calibration::Result::Next: + if (progress_data.has_progress) { + std::cout << " Progress: " << progress_data.progress << std::endl; + } + if (progress_data.has_status_text) { + std::cout << " Instruction: " << progress_data.status_text << std::endl; + } + break; + default: + std::cout << "--- Calibration failed with message: " << result << std::endl; + calibration_promise.set_value(); + break; + } + }; } void calibrate_gyro(mavsdk::Calibration &calibration) @@ -230,7 +230,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl m_has_flying_capability = hasFlyingCapability(m_mav_type); std::cout << (m_has_flying_capability ? "Connected to a flying vehicle" : "Connected to a non flying vehicle") - << std::endl; + << std::endl; m_action = std::make_shared(m_system); m_telemetry = std::make_shared(m_system); @@ -253,7 +253,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { if (m_system == NULL) { return false; - } else { + } + else { return true; } } @@ -272,7 +273,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } } return actual_address; - } else { + } + else { std::cout << "ERROR : The address parameter must start with \"serial:\" or \"udp:\" or \"tcp:\"." << std::endl; return std::string(); } @@ -288,8 +290,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { auto quat = m_telemetry.get()->attitude_quaternion(); auto posvel = m_telemetry.get()->position_velocity_ned(); - vpQuaternionVector q{quat.x, quat.y, quat.z, quat.w}; - vpTranslationVector t{posvel.position.north_m, posvel.position.east_m, posvel.position.down_m}; + vpQuaternionVector q { quat.x, quat.y, quat.z, quat.w }; + vpTranslationVector t { posvel.position.north_m, posvel.position.east_m, posvel.position.down_m }; ned_M_frd.buildFrom(t, q); } @@ -306,7 +308,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::tuple getHome() const { auto position = m_telemetry.get()->home(); - return {float(position.latitude_deg), float(position.longitude_deg)}; + return { float(position.latitude_deg), float(position.longitude_deg) }; } bool sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps) @@ -321,7 +323,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl flu_M_frd[2][2] = -1; vpHomogeneousMatrix enu_M_frd = enu_M_flu * flu_M_frd; - auto mocap = mavsdk::Mocap{m_system}; + auto mocap = mavsdk::Mocap { m_system }; mavsdk::Mocap::VisionPositionEstimate pose_estimate; vpHomogeneousMatrix ned_M_frd = vpMath::enu2ned(enu_M_frd); @@ -342,17 +344,18 @@ class vpRobotMavsdk::vpRobotMavsdkImpl if (set_position_result != mavsdk::Mocap::Result::Success) { std::cerr << "Set position failed: " << set_position_result << '\n'; return false; - } else { + } + else { if (display_fps > 0) { double display_time_ms = 1000. / display_fps; if (vpTime::measureTimeMs() - time_prev > display_time_ms) { time_prev = vpTime::measureTimeMs(); std::cout << "Send ned_M_frd MoCap data: " << std::endl; std::cout << "Translation [m]: " << pose_estimate.position_body.x_m << " , " - << pose_estimate.position_body.y_m << " , " << pose_estimate.position_body.z_m << std::endl; + << pose_estimate.position_body.y_m << " , " << pose_estimate.position_body.z_m << std::endl; std::cout << "Roll [rad]: " << pose_estimate.angle_body.roll_rad - << " , Pitch [rad]: " << pose_estimate.angle_body.pitch_rad - << " , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad << " ." << std::endl; + << " , Pitch [rad]: " << pose_estimate.angle_body.pitch_rad + << " , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad << " ." << std::endl; } } return true; @@ -363,7 +366,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { if (altitude > 0) { m_takeoffAlt = altitude; - } else { + } + else { std::cerr << "ERROR : The take off altitude must be positive." << std::endl; } } @@ -406,7 +410,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl bool setGPSGlobalOrigin(double latitude, double longitude, double altitude) { - auto passthrough = mavsdk::MavlinkPassthrough{m_system}; + auto passthrough = mavsdk::MavlinkPassthrough { m_system }; mavlink_set_gps_global_origin_t gps_global_origin; gps_global_origin.latitude = latitude * 10000000; gps_global_origin.longitude = longitude * 10000000; @@ -430,7 +434,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { - const mavsdk::Offboard::VelocityBodyYawspeed stay{}; + const mavsdk::Offboard::VelocityBodyYawspeed stay {}; m_offboard.get()->set_velocity_body(stay); mavsdk::Offboard::Result offboard_result = m_offboard.get()->start(); @@ -438,7 +442,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::cerr << "Offboard mode failed: " << offboard_result << std::endl; return false; } - } else if (m_verbose) { + } + else if (m_verbose) { std::cout << "Already in offboard mode" << std::endl; } @@ -474,15 +479,17 @@ class vpRobotMavsdk::vpRobotMavsdkImpl if (!interactive) { authorize_takeoff = true; - } else { + } + else { if (m_telemetry.get()->flight_mode() == mavsdk::Telemetry::FlightMode::Offboard) { authorize_takeoff = true; - } else { + } + else { std::string answer; while (answer != "Y" && answer != "y" && answer != "N" && answer != "n") { std::cout << "Current flight mode is not the offboard mode. Do you " - "want to force offboard mode ? (y/n)" - << std::endl; + "want to force offboard mode ? (y/n)" + << std::endl; std::cin >> answer; if (answer == "Y" || answer == "y") { authorize_takeoff = true; @@ -494,8 +501,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl if (m_telemetry.get()->in_air()) { std::cerr << "Cannot take off as the robot is already flying." << std::endl; return true; - } else if (authorize_takeoff) { - // Arm vehicle + } + else if (authorize_takeoff) { + // Arm vehicle if (!arm()) { return false; } @@ -525,11 +533,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl // Start off-board or guided mode takeControl(); - auto in_air_promise = std::promise{}; + auto in_air_promise = std::promise {}; auto in_air_future = in_air_promise.get_future(); mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry(); - vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w}; + vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w }; vpRotationMatrix R(q); vpRxyzVector rxyz(R); @@ -540,7 +548,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::cout << "Takeoff using position NED." << std::endl; - mavsdk::Offboard::PositionNedYaw takeoff{}; + mavsdk::Offboard::PositionNedYaw takeoff {}; takeoff.north_m = X_init; takeoff.east_m = Y_init; takeoff.down_m = Z_init - m_takeoffAlt; @@ -577,7 +585,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return false; } // Add check with Altitude - auto takeoff_finished_promise = std::promise{}; + auto takeoff_finished_promise = std::promise {}; auto takeoff_finished_future = takeoff_finished_promise.get_future(); #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) @@ -608,8 +616,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl #endif return false; } - } else { - // GPS connected, we use Action::takeoff() + } + else { + // GPS connected, we use Action::takeoff() std::cout << "---- DEBUG: GPS detected: use action::takeoff()" << std::endl; mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry(); @@ -622,7 +631,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return false; } - auto in_air_promise = std::promise{}; + auto in_air_promise = std::promise {}; auto in_air_future = in_air_promise.get_future(); #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state( @@ -653,7 +662,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl #endif } // Add check with Altitude - auto takeoff_finished_promise = std::promise{}; + auto takeoff_finished_promise = std::promise {}; auto takeoff_finished_future = takeoff_finished_promise.get_future(); #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) @@ -708,18 +717,17 @@ class vpRobotMavsdk::vpRobotMavsdkImpl takeControl(); mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry(); - vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w}; + vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w }; vpRotationMatrix R(q); vpRxyzVector rxyz(R); double X_init = odom.position_body.x_m; double Y_init = odom.position_body.y_m; - double Z_init = odom.position_body.z_m; double yaw_init = vpMath::deg(rxyz[2]); std::cout << "Landing using position NED." << std::endl; - mavsdk::Offboard::PositionNedYaw landing{}; + mavsdk::Offboard::PositionNedYaw landing {}; landing.north_m = X_init; landing.east_m = Y_init; landing.down_m = 0.; @@ -729,7 +737,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl bool success = false; // Add check with Altitude - auto landing_finished_promise = std::promise{}; + auto landing_finished_promise = std::promise {}; auto landing_finished_future = landing_finished_promise.get_future(); #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) @@ -791,7 +799,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl bool setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking, int timeout_sec) { - mavsdk::Offboard::PositionNedYaw position_target{}; + mavsdk::Offboard::PositionNedYaw position_target {}; position_target.north_m = ned_north; position_target.east_m = ned_east; @@ -799,7 +807,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl position_target.yaw_deg = vpMath::deg(ned_yaw); std::cout << "NED Pos to reach: " << position_target.north_m << " " << position_target.east_m << " " - << position_target.down_m << " " << position_target.yaw_deg << std::endl; + << position_target.down_m << " " << position_target.yaw_deg << std::endl; m_offboard.get()->set_position_ned(position_target); if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { @@ -811,13 +819,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl if (blocking) { // Add check with Altitude - auto position_reached_promise = std::promise{}; + auto position_reached_promise = std::promise {}; auto position_reached_future = position_reached_promise.get_future(); #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) auto handle_odom = m_telemetry.get()->subscribe_odometry( [this, &position_reached_promise, &handle, &position_target](mavsdk::Telemetry::Odometry odom) { - vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w}; + vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w }; vpRotationMatrix R(q); vpRxyzVector rxyz(R); double odom_yaw = vpMath::deg(rxyz[2]); @@ -834,7 +842,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl #else m_telemetry.get()->subscribe_odometry( [this, &position_reached_promise, &position_target](mavsdk::Telemetry::Odometry odom) { - vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w}; + vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w }; vpRotationMatrix R(q); vpRxyzVector rxyz(R); double odom_yaw = vpMath::deg(rxyz[2]); @@ -864,7 +872,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { mavsdk::Telemetry::Odometry odom; mavsdk::Telemetry::EulerAngle angles; - mavsdk::Offboard::PositionNedYaw position_target{}; + mavsdk::Offboard::PositionNedYaw position_target {}; position_target.north_m = ned_delta_north; position_target.east_m = ned_delta_east; @@ -928,7 +936,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } return false; } - mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{}; + mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {}; velocity_comm.forward_m_s = frd_vel_cmd[0]; velocity_comm.right_m_s = frd_vel_cmd[1]; velocity_comm.down_m_s = frd_vel_cmd[2]; @@ -958,7 +966,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::cerr << "Hold failed: " << hold_result << std::endl; return false; } - } else { + } + else { if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { if (m_verbose) { std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; @@ -981,7 +990,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return false; } - const mavsdk::Offboard::VelocityBodyYawspeed stay{}; + const mavsdk::Offboard::VelocityBodyYawspeed stay {}; m_offboard.get()->set_velocity_body(stay); return true; @@ -1008,7 +1017,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } return false; } - mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{}; + mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {}; velocity_comm.forward_m_s = 0.0; velocity_comm.right_m_s = 0.0; velocity_comm.down_m_s = 0.0; @@ -1027,7 +1036,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return false; } - mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{}; + mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {}; velocity_comm.forward_m_s = body_frd_vx; velocity_comm.right_m_s = 0.0; velocity_comm.down_m_s = 0.0; @@ -1045,7 +1054,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } return false; } - mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{}; + mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {}; velocity_comm.forward_m_s = 0.0; velocity_comm.right_m_s = body_frd_vy; velocity_comm.down_m_s = 0.0; @@ -1063,7 +1072,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } return false; } - mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{}; + mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {}; velocity_comm.forward_m_s = 0.0; velocity_comm.right_m_s = 0.0; velocity_comm.down_m_s = body_frd_vz; @@ -1091,24 +1100,24 @@ class vpRobotMavsdk::vpRobotMavsdkImpl private: //*** Attributes ***// - std::string m_address{}; ///< Ip address of the robot to discover on the network - mavsdk::Mavsdk m_mavsdk{}; + std::string m_address {}; ///< Ip address of the robot to discover on the network + mavsdk::Mavsdk m_mavsdk {}; std::shared_ptr m_system; std::shared_ptr m_action; std::shared_ptr m_telemetry; std::shared_ptr m_offboard; - double m_takeoffAlt{1.0}; ///< The altitude to aim for when calling the function takeoff + double m_takeoffAlt { 1.0 }; ///< The altitude to aim for when calling the function takeoff - MAV_TYPE m_mav_type{}; // Vehicle type - bool m_has_flying_capability{false}; + MAV_TYPE m_mav_type {}; // Vehicle type + bool m_has_flying_capability { false }; - bool m_system_ready{false}; - float m_position_incertitude{0.05}; - float m_yaw_incertitude{0.09}; // 5 deg - bool m_verbose{false}; - bool m_auto_land{true}; -}; + bool m_system_ready { false }; + float m_position_incertitude { 0.05 }; + float m_yaw_incertitude { 0.09 }; // 5 deg + bool m_verbose { false }; + bool m_auto_land { true }; + }; #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS /*! @@ -1270,7 +1279,7 @@ std::tuple vpRobotMavsdk::getHome() const { return m_impl->getHome * * \warning Should be executed only when the vehicle is on a flat surface. */ -void vpRobotMavsdk::doFlatTrim() {} +void vpRobotMavsdk::doFlatTrim() { } /*! * Sets the take off altitude. @@ -1298,6 +1307,8 @@ bool vpRobotMavsdk::disarm() { return m_impl->disarm(); } * \param[in] interactive : If true asks the user if the offboard mode is to be forced through the terminal. If false * offboard mode is automatically set. * \param[in] timeout_sec : Time out in seconds to achieve takeoff. + * \param[in] use_gps : When GPS is to use, set this flag to true. Set to false otherwise. + * * \return * - If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, * typically when a timeout occurs. If the vehicle has flying capabilities and is already flying, return true. @@ -1316,6 +1327,7 @@ bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec, bool use_gps) * offboard mode is automatically set. * \param[in] takeoff_altitude : Take off altitude in [m]. Should be a positive value. * \param[in] timeout_sec : Time out in seconds to achieve takeoff. + * \param[in] use_gps : When GPS is to use, set this flag to true. Set to false otherwise. * \return * - If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, * typically when a timeout occurs. @@ -1581,5 +1593,5 @@ bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability() #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpRobotMavsdk.cpp.o) has no // symbols -void dummy_vpRobotMavsdk(){}; +void dummy_vpRobotMavsdk() { }; #endif diff --git a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp index 457735cc62..f62aecad44 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp @@ -121,7 +121,7 @@ void emergencyStopViper650(int signo) The only available constructor. - This contructor calls init() to initialise the connection with the + This constructor calls init() to initialise the connection with the MotionBox or low level controller, send the default \f$^e{\bf M}_c\f$ homogeneous matrix and power on the robot. diff --git a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp index 2d6d369b8d..6ebdf0cf3e 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp @@ -121,7 +121,7 @@ void emergencyStopViper850(int signo) The only available constructor. - This contructor calls init() to initialise the connection with the + This constructor calls init() to initialise the connection with the MotionBox or low level controller, send the default \f$^e{\bf M}_c\f$ homogeneous matrix and power on the robot. diff --git a/modules/tracker/mbt/src/vpMbtPolygon.cpp b/modules/tracker/mbt/src/vpMbtPolygon.cpp index bd11485cfa..4ea7c8485c 100644 --- a/modules/tracker/mbt/src/vpMbtPolygon.cpp +++ b/modules/tracker/mbt/src/vpMbtPolygon.cpp @@ -62,7 +62,7 @@ vpMbtPolygon::vpMbtPolygon(const vpMbtPolygon &mbtp) minLineLengthThresh(mbtp.minLineLengthThresh), minPolygonAreaThresh(mbtp.minPolygonAreaThresh), name(mbtp.name), hasOrientation(mbtp.hasOrientation) { - //*this = mbtp; // Should not be called by copy contructor to avoid multiple + //*this = mbtp; // Should not be called by copy constructor to avoid multiple // assignements. } diff --git a/modules/tracker/me/include/visp3/me/vpMe.h b/modules/tracker/me/include/visp3/me/vpMe.h index 4f89335181..48d204005d 100644 --- a/modules/tracker/me/include/visp3/me/vpMe.h +++ b/modules/tracker/me/include/visp3/me/vpMe.h @@ -446,10 +446,10 @@ class VISP_EXPORT vpMe #ifdef VISP_HAVE_NLOHMANN_JSON /*! - * @brief Convert a vpMe object to a JSON representation + * @brief Convert a vpMe object to a JSON representation. * - * @param j resulting json object - * @param me the object to convert + * @param j : Resulting json object. + * @param me : The object to convert. */ friend void to_json(nlohmann::json &j, const vpMe &me); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h index 360c495876..d8d6ce9134 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h @@ -291,7 +291,7 @@ class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature cMo } vpFeatureTranslationRepresentationType; - // basic contructor + // basic constructor vpFeatureTranslation(); // basic constructor specifying the type of translation feature explicit vpFeatureTranslation(vpFeatureTranslationRepresentationType r); diff --git a/modules/vs/include/visp3/vs/vpAdaptiveGain.h b/modules/vs/include/visp3/vs/vpAdaptiveGain.h index 66f8c8a1d3..43020522b4 100644 --- a/modules/vs/include/visp3/vs/vpAdaptiveGain.h +++ b/modules/vs/include/visp3/vs/vpAdaptiveGain.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,93 +29,89 @@ * * Description: * Adaptive gain. - * - * Authors: - * Nicolas Mansard - * -*****************************************************************************/ -/*! -\file vpAdaptiveGain.h -\brief Adaptive gain -*/ + */ #ifndef _vpAdaptiveGain_h_ #define _vpAdaptiveGain_h_ +/*! + * \file vpAdaptiveGain.h + * \brief Adaptive gain + */ + #include #include class vpColVector; -/*! - \class vpAdaptiveGain - - \ingroup group_task - - \brief Adaptive gain computation. - - As described in \cite Kermorgant14a, a varying gain \f$ \lambda \f$ could be -used in the visual servoing control law \f[{\bf v}_c = -\lambda {\bf -L}^{+}_{e} {\bf e}\f] with - - \f[ \lambda (|| {\bf e}||) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ -\lambda'_0}{\lambda_0 - \lambda_\infty}||{\bf e}||} + \lambda_\infty \f] - - where: - - - \f$\lambda_0 = \lambda(0)\f$ is the gain in 0, that is for very small -values of \f$||{\bf e}||\f$ - - \f$\lambda_\infty = \lambda_{||{\bf e}|| \rightarrow \infty}\lambda(||{\bf -e}||)\f$ is the gain to infinity, that is for very high values of \f$||{\bf -e}||\f$ - - \f$\lambda'_0\f$ is the slope of \f$\lambda\f$ at \f$||{\bf e}|| = 0\f$ - - As described in \ref tutorial-boost-vs, the interest of \ref adaptive_gain -is to reduce the time to convergence in order to speed up the servo. - - The following example shows how to use this class in order to use an -adaptive gain with the following parameters \f$\lambda_0 = 4\f$, -\f$\lambda_\infty = 0.4 \f$ and \f$\lambda'_0 = 30\f$. - -\code -#include -#include - -int main() -{ - vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30 - - vpServo servo; - servo.setLambda(lambda); - - while(1) { - - vpColVector v = servo.computeControlLaw(); - } -} - \endcode - - This other example shows how to use this class in order to set a constant -gain \f$\lambda = 0.5\f$ that will ensure an exponential decrease of the task -error. - -\code -#include -#include - -int main() -{ - vpAdaptiveGain lambda(0.5); - - vpServo servo; - servo.setLambda(lambda); - while(1) { - - vpColVector v = servo.computeControlLaw(); - } -} - \endcode -*/ +/*! + * \class vpAdaptiveGain + * + * \ingroup group_task + * + * \brief Adaptive gain computation. + * + * As described in \cite Kermorgant14a, a varying gain \f$ \lambda \f$ could be + * used in the visual servoing control law \f[{\bf v}_c = -\lambda {\bf + * L}^{+}_{e} {\bf e}\f] with + * + * \f[ \lambda (|| {\bf e}||) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ + * \lambda'_0}{\lambda_0 - \lambda_\infty}||{\bf e}||} + \lambda_\infty \f] + * + * where: + * + * - \f$\lambda_0 = \lambda(0)\f$ is the gain in 0, that is for very small + * values of \f$||{\bf e}||\f$ + * - \f$\lambda_\infty = \lambda_{||{\bf e}|| \rightarrow \infty}\lambda(||{\bf + * e}||)\f$ is the gain to infinity, that is for very high values of \f$||{\bf + * e}||\f$ + * - \f$\lambda'_0\f$ is the slope of \f$\lambda\f$ at \f$||{\bf e}|| = 0\f$ + * + * As described in \ref tutorial-boost-vs, the interest of \ref adaptive_gain + * is to reduce the time to convergence in order to speed up the servo. + * + * The following example shows how to use this class in order to use an + * adaptive gain with the following parameters \f$\lambda_0 = 4\f$, + * \f$\lambda_\infty = 0.4 \f$ and \f$\lambda'_0 = 30\f$. + * + * \code + * #include + * #include + * + * int main() + * { + * vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30 + * + * vpServo servo; + * servo.setLambda(lambda); + * + * while(1) { + * vpColVector v = servo.computeControlLaw(); + * } + * } + * \endcode + * + * This other example shows how to use this class in order to set a constant + * gain \f$\lambda = 0.5\f$ that will ensure an exponential decrease of the task + * error. + * + * \code + * #include + * #include + * + * int main() + * { + * vpAdaptiveGain lambda(0.5); + * + * vpServo servo; + * servo.setLambda(lambda); + * + * while(1) { + * vpColVector v = servo.computeControlLaw(); + * } + * } + * \endcode + */ class VISP_EXPORT vpAdaptiveGain { @@ -135,74 +130,176 @@ class VISP_EXPORT vpAdaptiveGain mutable double lambda; public: - /* --- CONSTRUCTOR -------------------------------------------------------- - */ - + /*! + * Basic constructor which initializes all the parameters with their default + * value: + * - \f$ \lambda_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO + * - \f$ \lambda_\infty = 0.1666 \f$ using + * vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY + * - \f$ \lambda'_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE + */ vpAdaptiveGain(); + + /*! + * Constructor that initializes the gain as constant. In that case + * \f$\lambda(||{\bf e}||) = c\f$. + * + * \param c : Value of the constant gain. A typical value is 0.5. + */ explicit vpAdaptiveGain(double c); + + /*! + * Constructor that initializes the gain as adaptive. + * + * \param gain_at_zero : the expected gain when \f$||{\bf e}||=0\f$: + * \f$\lambda_0\f$. + * \param gain_at_infinity : the expected gain when \f$||{\bf + * e}||\rightarrow\infty\f$: \f$\lambda_\infty\f$. + * \param slope_at_zero : the + * expected slope of \f$\lambda(||{\bf e}||)\f$ when \f$||{\bf e}||=0\f$: + * \f$\lambda'_0\f$. + */ vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero); - /* --- INIT --------------------------------------------------------------- + /*! + * Initializes the parameters to have a constant gain. In that case + * \f$\lambda(||{\bf e}||) = c\f$. + * + * \param c : Value of the constant gain. A typical value is 0.5. */ void initFromConstant(double c); + + /*! + * Initializes the parameters with the default value : + * - \f$ \lambda_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO + * - \f$ \lambda_\infty = 0.1666 \f$ using + * vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY + * - \f$ \lambda'_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE + */ void initFromVoid(void); + + /*! + * Set the parameters \f$\lambda_0, \lambda_\infty, \lambda'_0\f$ used to + * compute \f$\lambda(||{\bf e}||)\f$. + * + * \param gain_at_zero : the expected gain when \f$||{\bf e}||=0\f$: + * \f$\lambda_0\f$. + * \param gain_at_infinity : the expected gain when \f$||{\bf + * e}||\rightarrow\infty\f$: \f$\lambda_\infty\f$. + * \param slope_at_zero : the + * expected slope of \f$\lambda(||{\bf e}||)\f$ when \f$||{\bf e}||=0\f$: + * \f$\lambda'_0\f$. + */ void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero); - /* --- MODIFIORS ---------------------------------------------------------- + /*! + * Sets the internal parameters in order to obtain a constant gain equal to + * the gain in 0 set through the parameter \f$\lambda_0\f$. + * + * \return It returns the value of the constant gain \f$\lambda_0\f$. */ double setConstant(void); - /* --- COMPUTE ------------------------------------------------------------ - */ - /* \brief Calcule la valeur de lambda au point courrant. - * - * Determine la valeur du lambda adaptatif en fonction de la valeur - * de la norme de la fonction de tache e par extrapolation exponentielle. - * La fonction est : (en_infini - en_zero) * exp (-pente * ||e|| ) + - * en_infini. On a bien : - * - lambda(10^5) = en_infini ; - * - lambda(0) = en_zero ; - * - lambda(x ~ 0) ~ - pente * x + en_zero. - * \param val_e: valeur de la norme de l'erreur. - * \return: valeur de gain au point courrant. + /*! + * Computes the value of the adaptive gain \f$\lambda(x)\f$ using: + * + * \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ + * \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f] + * + * \param x : Input value to consider. During a visual servo this value can be + * the Euclidean norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf + * e}||_{\infty}\f$ of the task function. + * + * \return It returns the value of the computed gain. */ double value_const(double x) const; - /* \brief Calcule la valeur de lambda au point courrant et stockage du - * resultat. + /*! + * Computes the value of the adaptive gain \f$\lambda(x)\f$ using: + * + * \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ + * \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f] + * + * This value is stored as a parameter of the class. * - * La fonction calcule la valeur de lambda d'apres la valeur de la norme - * de l'erreur, comme le fait la fonction valeur_const. - * La fonction non constante stocke de plus le resultat dans this ->lambda. - * \param val_e: valeur de la norme de l'erreur. - * \return: valeur de gain au point courrant. + * \param x : Input value to consider. During a visual servo this value can be + * the Euclidean norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf + * e}||_{\infty}\f$ of the task function. + * + * \return It returns the value of the computed gain. */ double value(double x) const; + /*! + * Gets the value of the gain at infinity (ie the value of \f$ \lambda_\infty = + * c \f$). This function is similar to limitValue() except that here the value + * is not stored as a parameter of the class. + * + * \return It returns the value of the gain at infinity. + */ double limitValue_const(void) const; - double limitValue(void) const; - - /* --- ACCESSORS ---------------------------------------------------------- + /*! + * Gets the value of the gain at infinity (ie the value of \f$\lambda_\infty = + * c \f$) and stores it as a parameter of the class. + * + * \return It returns the value of the gain at infinity. */ + double limitValue(void) const; /*! - Gets the last adaptive gain value which was stored in the class. - - \return It returns the last adaptive gain value which was stored in the - class. - */ + * Gets the last adaptive gain value which was stored in the class. + * + * \return It returns the last adaptive gain value which was stored in the + * class. + */ inline double getLastValue(void) const { return this->lambda; } + /*! + * Operator that computes \f$\lambda(x)\f$ where + * + * \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ + * \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f] + * + * \param x : Input value to consider. During a visual servo this value can be + * the Euclidean norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf + * e}||_{\infty}\f$ of the task function. + * + * \return It returns the value of the computed gain. + * + * \sa value() + */ double operator()(double x) const; - /* \brief Lance la fonction valeur avec la norme INFINIE du vecteur. */ + /*! + * Operator which computes \f$\lambda({||x||}_{\infty})\f$ where + * + * \f[ \lambda ({||x||}_{\infty}) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ + * \lambda'_0}{\lambda_0 - \lambda_\infty}{||x||}_{\infty}} + \lambda_\infty + * \f] + * + * \param x : Input vector \f$ \bf x\f$ to consider. + * + * \return It returns the value of the computed gain. + */ double operator()(const vpColVector &x) const; - /* \brief Idem function limitValue. */ + /*! + * Gets the value of the gain at infinity (ie the value of \f$\lambda_\infty = + * c \f$). + * + * \return It returns the value of the gain at infinity. + * + * \sa limitValue() + */ double operator()(void) const; - /* --- IOSTREAM ----------------------------------------------------------- + /*! + * Prints the adaptive gain parameters \f$\lambda_0, \lambda_\infty, + * \lambda'_0\f$. + * + * \param os : The stream where to print the adaptive gain parameters. + * \param lambda : The adaptive gain containing the parameters to print. */ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda); }; diff --git a/modules/vs/include/visp3/vs/vpServo.h b/modules/vs/include/visp3/vs/vpServo.h index 60d0a62216..6e8c82dc81 100644 --- a/modules/vs/include/visp3/vs/vpServo.h +++ b/modules/vs/include/visp3/vs/vpServo.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,16 +29,15 @@ * * Description: * Visual servoing control law. - * -*****************************************************************************/ + */ -#ifndef vpServo_H -#define vpServo_H +#ifndef _vpServo_h_ +#define _vpServo_h_ /*! - \file vpServo.h - \brief Class required to compute the visual servoing control law. -*/ + * \file vpServo.h + * \brief Class required to compute the visual servoing control law. + */ #include @@ -50,166 +48,200 @@ #include /*! - \class vpServo - - \ingroup group_task - Class required to compute the visual servoing control law described - in \cite Chaumette06a and \cite Chaumette07a. - - \warning To avoid potential memory leaks, it is mandatory to call - explicitly the kill() function to destroy the task. Otherwise, the - destructor ~vpServo() launch an exception - vpServoException::notKilledProperly. - - To learn how to use this class, we suggest first to follow the \ref - tutorial-ibvs. The \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs - are also useful for advanced usage of this class. - - The example below shows how to build a position-based visual servo - from 3D visual features \f$s=({^{c^*}}t_c,\theta u)\f$. In that - case, we have \f$s^* = 0\f$. Let us denote \f$\theta u\f$ the angle/axis - parametrization of the rotation \f${^{c^*}}R_c\f$. Moreover,\f$ - {^{c^*}}t_c\f$ and \f${^{c^*}}R_c\f$ represent respectively the - translation and the rotation between the desired camera frame and - the current one obtained by pose estimation (see vpPose class). - - \code - #include - #include - #include - #include - #include - #include - - int main() - { - // Creation of an homogeneous matrix that represent the displacement - // the camera has to achieve to move from the desired camera frame - // and the current one - vpHomogeneousMatrix cdMc; - - // ... cdMc is here the result of a pose estimation - - // Creation of the current visual feature s = (c*_t_c, ThetaU) - vpFeatureTranslation s_t(vpFeatureTranslation::cdMc); - vpFeatureThetaU s_tu(vpFeatureThetaU::cdRc); - // Set the initial values of the current visual feature s = (c*_t_c, ThetaU) - s_t.buildFrom(cdMc); - s_tu.buildFrom(cdMc); - - // Build the desired visual feature s* = (0,0) - vpFeatureTranslation s_star_t(vpFeatureTranslation::cdMc); // Default initialization to zero - vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc); // Default initialization to zero - - vpColVector v; // Camera velocity - double error; // Task error - - // Creation of the visual servo task. - vpServo task; - - // Visual servo task initialization - // - Camera is mounted on the robot end-effector and velocities are - // computed in the camera frame - task.setServo(vpServo::EYEINHAND_CAMERA); - // - Interaction matrix is computed with the current visual features s - task.setInteractionMatrixType(vpServo::CURRENT); - // - Set the constant gain to 1 - task.setLambda(1); - // - Add current and desired translation feature - task.addFeature(s_t, s_star_t); - // - Add current and desired ThetaU feature for the rotation - task.addFeature(s_tu, s_star_tu); - - // Visual servoing loop. The objective is here to update the visual - // features s = (c*_t_c, ThetaU), compute the control law and apply - // it to the robot - do { - // ... cdMc is here the result of a pose estimation - - // Update the current visual feature s - s_t.buildFrom(cdMc); // Update translation visual feature - s_tu.buildFrom(cdMc); // Update ThetaU visual feature - - v = task.computeControlLaw(); // Compute camera velocity skew - error = ( task.getError() ).sumSquare(); // error = s^2 - s_star^2 - } while (error > 0.0001); // Stop the task when current and desired visual features are close - } - \endcode - -*/ - + * \class vpServo + * + * \ingroup group_task + * Class required to compute the visual servoing control law described + * in \cite Chaumette06a and \cite Chaumette07a. + * + * \warning To avoid potential memory leaks, it is mandatory to call + * explicitly the kill() function to destroy the task. Otherwise, the + * destructor ~vpServo() launch an exception + * vpServoException::notKilledProperly. + * + * To learn how to use this class, we suggest first to follow the \ref + * tutorial-ibvs. The \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs + * are also useful for advanced usage of this class. + * + * The example below shows how to build a position-based visual servo + * from 3D visual features \f$s=({^{c^*}}t_c,\theta u)\f$. In that + * case, we have \f$s^* = 0\f$. Let us denote \f$\theta u\f$ the angle/axis + * parametrization of the rotation \f${^{c^*}}R_c\f$. Moreover,\f$ + * {^{c^*}}t_c\f$ and \f${^{c^*}}R_c\f$ represent respectively the + * translation and the rotation between the desired camera frame and + * the current one obtained by pose estimation (see vpPose class). + * + * \code + * #include + * #include + * #include + * #include + * #include + * #include + * + * int main() + * { + * // Creation of an homogeneous matrix that represent the displacement + * // the camera has to achieve to move from the desired camera frame + * // and the current one + * vpHomogeneousMatrix cdMc; + * + * // ... cdMc is here the result of a pose estimation + * + * // Creation of the current visual feature s = (c*_t_c, ThetaU) + * vpFeatureTranslation s_t(vpFeatureTranslation::cdMc); + * vpFeatureThetaU s_tu(vpFeatureThetaU::cdRc); + * // Set the initial values of the current visual feature s = (c*_t_c, ThetaU) + * s_t.buildFrom(cdMc); + * s_tu.buildFrom(cdMc); + * + * // Build the desired visual feature s* = (0,0) + * vpFeatureTranslation s_star_t(vpFeatureTranslation::cdMc); // Default initialization to zero + * vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc); // Default initialization to zero + * + * vpColVector v; // Camera velocity + * double error; // Task error + * + * // Creation of the visual servo task. + * vpServo task; + * + * // Visual servo task initialization + * // - Camera is mounted on the robot end-effector and velocities are + * // computed in the camera frame + * task.setServo(vpServo::EYEINHAND_CAMERA); + * // - Interaction matrix is computed with the current visual features s + * task.setInteractionMatrixType(vpServo::CURRENT); + * // - Set the constant gain to 1 + * task.setLambda(1); + * // - Add current and desired translation feature + * task.addFeature(s_t, s_star_t); + * // - Add current and desired ThetaU feature for the rotation + * task.addFeature(s_tu, s_star_tu); + * + * // Visual servoing loop. The objective is here to update the visual + * // features s = (c*_t_c, ThetaU), compute the control law and apply + * // it to the robot + * do { + * // ... cdMc is here the result of a pose estimation + * + * // Update the current visual feature s + * s_t.buildFrom(cdMc); // Update translation visual feature + * s_tu.buildFrom(cdMc); // Update ThetaU visual feature + * + * v = task.computeControlLaw(); // Compute camera velocity skew + * error = ( task.getError() ).sumSquare(); // error = s^2 - s_star^2 + * } while (error > 0.0001); // Stop the task when current and desired visual features are close + * } + * \endcode + */ class VISP_EXPORT vpServo { - /* - Choice of the visual servoing control law - */ + public: + /*! + * Choice of the visual servoing control law. + */ typedef enum { + /*! + * No control law is specified. + */ NONE, - /*!< No control law is specified. */ + /*! + * Eye in hand visual servoing with the following control law + * \f[{\bf v}_c = -\lambda {\widehat {\bf L}}^{+}_{e} {\bf e}\f] + * where camera velocities are computed. + */ EYEINHAND_CAMERA, - /*!< Eye in hand visual servoing with the following control law - \f[{\bf v}_c = -\lambda {\widehat {\bf L}}^{+}_{e} {\bf e}\f] - where camera velocities are computed. */ + /*! + * Eye in hand visual servoing with the following control law + * \f[{\dot {\bf q}} = -\lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf + * V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are + * computed. + */ EYEINHAND_L_cVe_eJe, - /*!< Eye in hand visual servoing with the following control law - \f[{\dot {\bf q}} = -\lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf - V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are - computed. */ + /*! + * Eye to hand visual servoing with the following control law + * \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e + * {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are + * computed. + */ EYETOHAND_L_cVe_eJe, - /*!< Eye to hand visual servoing with the following control law - \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e - {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are - computed. */ + /*! + * Eye to hand visual servoing with the following control law + * \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f + * {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint + * velocities are computed. + */ EYETOHAND_L_cVf_fVe_eJe, - /*!< Eye to hand visual servoing with the following control law - \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f - {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint - velocities are computed. */ + /*! + * Eye to hand visual servoing with the following control law + * \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f + * {^f}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are + * computed. + */ EYETOHAND_L_cVf_fJe - /*!< Eye to hand visual servoing with the following control law - \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f - {^f}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are - computed. */ } vpServoType; + /*! + * Choice of the interaction matrix type used in the visual servoing control law. + */ typedef enum { + /*! + * In the control law (see vpServo::vpServoType), uses the interaction + * matrix \f${\widehat {\bf L}}_s \f$computed using the current features + * \f$\bf s\f$. + */ CURRENT, - /*!< In the control law (see vpServo::vpServoType), uses the interaction - matrix \f${\widehat {\bf L}}_s \f$computed using the current features - \f$\bf s\f$. */ + /*! + * In the control law (see vpServo::vpServoType), uses the interaction + * matrix \f${\widehat {\bf L}}_{s^*} \f$computed using the desired + * features \f${\bf s}^*\f$. + */ DESIRED, - /*!< In the control law (see vpServo::vpServoType), uses the interaction - matrix \f${\widehat {\bf L}}_{s^*} \f$computed using the desired - features \f${\bf s}^*\f$. */ + /*! + * In the control law (see vpServo::vpServoType), uses the interaction + * matrix \f${\widehat {\bf L}} = \left({\widehat {\bf L}}_s + {\widehat + * {\bf L}}_{s^*}\right)/2 \f$. + */ MEAN, - /*!< In the control law (see vpServo::vpServoType), uses the interaction - matrix \f${\widehat {\bf L}} = \left({\widehat {\bf L}}_s + {\widehat - {\bf L}}_{s^*}\right)/2 \f$. */ + /*! + * In the control law (see vpServo::vpServoType), uses an interaction + * matrix set by the user. + */ USER_DEFINED - /*!< In the control law (see vpServo::vpServoType), uses an interaction - matrix set by the user. */ } vpServoIteractionMatrixType; + + /*! + * Choice of the interaction matrix inversion method. + */ typedef enum { - TRANSPOSE, /*!< In the control law (see vpServo::vpServoType), uses the - transpose instead of the pseudo inverse. */ - PSEUDO_INVERSE /*!< In the control law (see vpServo::vpServoType), uses - the pseudo inverse. */ + /*! + * In the control law (see vpServo::vpServoType), uses the + * transpose instead of the pseudo inverse. + */ + TRANSPOSE, + /*! + * In the control law (see vpServo::vpServoType), uses the pseudo inverse. + */ + PSEUDO_INVERSE } vpServoInversionType; + /*! + * Choice of the information to print. + */ typedef enum { - ALL, /*!< Print all the task information. */ - CONTROLLER, /*!< Print the type of controller law. */ - ERROR_VECTOR, /*!< Print the error vector \f$\bf e = (s-s^*)\f$. */ - FEATURE_CURRENT, /*!< Print the current features \f$\bf s\f$. */ - FEATURE_DESIRED, /*!< Print the desired features \f${\bf s}^*\f$. */ - GAIN, /*!< Print the gain \f$\lambda\f$. */ - INTERACTION_MATRIX, /*!< Print the interaction matrix. */ - MINIMUM /*!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. */ + ALL, //!< Print all the task information. + CONTROLLER, //!< Print the type of controller law. + ERROR_VECTOR, //!< Print the error vector \f$\bf e = (s-s^*)\f$. + FEATURE_CURRENT, //!< Print the current features \f$\bf s\f$. + FEATURE_DESIRED, //!< Print the desired features \f${\bf s}^*\f$. + GAIN, //!< Print the gain \f$\lambda\f$. + INTERACTION_MATRIX, //!< Print the interaction matrix. + MINIMUM //!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. } vpServoPrintType; // private: @@ -236,142 +268,683 @@ class VISP_EXPORT vpServo //#endif public: - // default constructor + /*! + * Default constructor that initializes the following settings: + * - No control law is specified. The user has to call setServo() to specify + * the control law. + * - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is + * computed with the desired features \f${\bf s}^*\f$. Using + * setInteractionMatrixType() you can also compute the interaction matrix with + * the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s + * + {\widehat {\bf L}}_{s^*}\right)/2\f$. + * - In the control law the pseudo inverse will be used. The method + * setInteractionMatrixType() allows to use the transpose instead. + * + * \warning By default the threshold used to compute the pseudo-inverse is set to 1e-6. + * Advanced user can modify this value using setPseudoInverseThreshold(). + */ vpServo(); - // constructor with Choice of the visual servoing control law - explicit vpServo(vpServoType servoType); - // destructor + + /*! + * Constructor that allows to choose the visual servoing control law. + * + * \param servo_type : Visual servoing control law. + * + * The other settings are the following: + * - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is + * computed with the desired features \f${\bf s}^*\f$. Using + * setInteractionMatrixType() you can also compute the interaction matrix with + * the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s + * + {\widehat {\bf L}}_{s^*}\right)/2\f$. + * - In the control law the pseudo inverse will be used. The method + * setInteractionMatrixType() allows to use the transpose instead. + */ + explicit vpServo(vpServoType servo_type); + + /*! + * Destructor. + * + * Since ViSP > 3.3.0 calls kill() to destroy the current and desired feature lists. + * + * \sa kill() + */ virtual ~vpServo(); - // create a new ste of two visual features - void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL); - // create a new ste of two visual features - void addFeature(vpBasicFeature &s, unsigned int select = vpBasicFeature::FEATURE_ALL); + /*! + * Add a new set of 2 features \f$\bf s\f$ and \f${\bf s}^*\f$ in the task. + * + * \param s_cur : Current visual feature denoted \f$\bf s\f$. + * \param s_star : Desired visual feature denoted \f${\bf s}^*\f$. + * \param select : Feature selector. By default all the features in \e s and \e + * s_star are used, but is is possible to specify which one is used in case of + * multiple features. + * + * The following sample code explain how to use this method to add a visual + * feature point \f$(x,y)\f$: + * \code + * vpFeaturePoint s, s_star; + * ... + * vpServo task; + * task.addFeature(s, s_star); + * \endcode + * + * For example to use only the \f$x\f$ visual feature, the previous code + * becomes: + * \code + * vpFeaturePoint s, s_star; + * ... + * vpServo task; + * task.addFeature(s, s_star, vpFeaturePoint::selectX()); + * \endcode + */ + void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL); + + /*! + * Add a new features \f$\bf s\f$ in the task. The desired visual feature + * denoted \f${\bf s}^*\f$ is equal to zero. + * + * \param s_cur : Current visual feature denoted \f$\bf s\f$. + * \param select : Feature selector. By default all the features in \e s are + * used, but is is possible to specify which one is used in case of multiple + * features. + * + * The following sample code explain how to use this method to add a \f$\theta + * {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature: + * \code + * vpFeatureThetaU s(vpFeatureThetaU::cRcd); + * ... + * vpServo task; + * task.addFeature(s); + * \endcode + * + * For example to use only the \f$\theta u_x\f$ feature, the previous code + * becomes: + * \code + * vpFeatureThetaU s(vpFeatureThetaU::cRcd); + * ... + * vpServo task; + * task.addFeature(s, vpFeatureThetaU::selectTUx); + * \endcode + */ + void addFeature(vpBasicFeature &s_cur, unsigned int select = vpBasicFeature::FEATURE_ALL); - // compute the desired control law + /*! + * Compute the control law specified using setServo(). See vpServo::vpServoType + * for more details concerning the control laws that are available. The \ref + * tutorial-ibvs and \ref tutorial-boost-vs are also useful to illustrate the + * usage of this function. + * + * The general form of the control law is the following: + * + * \f[ + * {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + * \f] + * + * where : + * - \f${\bf \dot q}\f$ is the resulting velocity command to apply to the + * robot. + * - the sign of the control law depends on the eye in hand or eye to hand + * configuration. + * - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction + * matrix and of the robot Jacobian. + * - \f$\bf e = (s-s^*)\f$ is the error to regulate. + * + * To ensure continuous sequencing the computeControlLaw(double) function can + * be used. It will ensure that the velocities that are computed are + * continuous. + */ vpColVector computeControlLaw(); - // compute the desired control law + + /*! + * Compute the control law specified using setServo(). See vpServo::vpServoType + * for more details concerning the control laws that are available. The \ref + * tutorial-boost-vs is also useful to illustrate the usage of this function. + * + * To the general form of the control law given in computeControlLaw(), we add + * here an additional term that comes from the task sequencing approach + * described in \cite Mansard07e equation (17). This additional term allows to + * compute continuous velocities by avoiding abrupt changes in the command. + * + * The form of the control law considered here is the following: + * + * \f[ + * {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf + * \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t) \f] + * + * where : + * - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply + * to the robot. + * - the sign of the control law depends on the eye in hand or eye to hand + * configuration. + * - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction + * matrix and of the robot Jacobian. + * - \f$\bf e = (s-s^*)\f$ is the error to regulate. + * - \f$t\f$ is the time given as parameter of this method. + * - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified + * using setMu(). + * - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf + * \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$. This value is internally stored + * either at the first call of this method, or when \e t parameter is set to 0. + * + * \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ + * {{\bf e}(0)}\f$ is refreshed internally. + */ vpColVector computeControlLaw(double t); + + /*! + * Compute the control law specified using setServo(). See vpServo::vpServoType + * for more details concerning the control laws that are available. + * + * To the general form of the control law given in computeControlLaw(), we add + * here an additional term that comes from the task sequencing approach + * described in \cite Mansard07e equation (17). This additional term allows to + * compute continuous velocities by avoiding abrupt changes in the command. + * + * The form of the control law considered here is the following: + * + * \f[ + * {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + \left({\bf \dot + * e}(0) \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\right) \exp(-\mu + * t) \f] + * + * where : + * - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply + * to the robot. + * - the sign of the control law depends on the eye in hand or eye to hand + * configuration. + * - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction + * matrix and of the robot Jacobian. + * - \f$\bf e = (s-s^*)\f$ is the error to regulate. + * - \f$t\f$ is the time given as parameter of this method. + * - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified + * using setMu(). + * - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf + * \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$. This value is internally stored + * either at the first call of this method, or when \e t parameter is set to 0. + * + * \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ + * {{\bf e}(0)}\f$ is refreshed internally. \param e_dot_init : Initial value + * of \f${\bf \dot e}(0)\f$. + */ vpColVector computeControlLaw(double t, const vpColVector &e_dot_init); - // compute the error between the current set of visual features and - // the desired set of visual features + /*! + * Compute the error \f$\bf e =(s - s^*)\f$ between the current set of visual + * features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$. + * + * \return The error vector \f$\bf e\f$. + */ vpColVector computeError(); - // compute the interaction matrix related to the set of visual features + + /*! + * Compute and return the interaction matrix related to the set of visual + * features. + * + * \return The interaction matrix \f${\widehat {\bf L}}_e\f$ used in the + * control law specified using setServo(). + */ vpMatrix computeInteractionMatrix(); - // Return the task dimension. + /*! + * Return the task dimension. + */ unsigned int getDimension() const; + /*! - Return the error \f$\bf e = (s - s^*)\f$ between the current set of visual - features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$. The - error vector is updated after a call of computeError() or computeControlLaw(). - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing - vpColVector e = task.getError(); // Get the error vector - \endcode + * Return the error \f$\bf e = (s - s^*)\f$ between the current set of visual + * features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$. The + * error vector is updated after a call of computeError() or computeControlLaw(). + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing + * vpColVector e = task.getError(); // Get the error vector + * \endcode */ inline vpColVector getError() const { return error; } /*! - Return the interaction matrix \f$L\f$ used to compute the task jacobian - \f$J_1\f$. The interaction matrix is updated after a call to - computeInteractionMatrix() or computeControlLaw(). - - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing vpMatrix - L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v - \endcode - \sa getTaskJacobian() + * Return the interaction matrix \f$L\f$ used to compute the task jacobian + * \f$J_1\f$. The interaction matrix is updated after a call to + * computeInteractionMatrix() or computeControlLaw(). + * + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing vpMatrix + * L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v + * \endcode + * \sa getTaskJacobian() */ inline vpMatrix getInteractionMatrix() const { return L; } - vpMatrix getI_WpW() const; /*! - Return the visual servo type. + * Return the projection operator \f${\bf I}-{\bf W}^+{\bf W}\f$. This + * operator is updated after a call of computeControlLaw(). + * + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing + * vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator + * \endcode + * \sa getWpW() + */ + vpMatrix getI_WpW() const { return I_WpW; } + + /*! + * Return the visual servo type. */ inline vpServoType getServoType() const { return servoType; } - vpMatrix getLargeP() const; + /*! + * Return the large projection operator. This operator is updated + * after a call of computeControlLaw(). + * + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing + * vpMatrix P = task.getP(); // Get the large projection operator + * \endcode + * \sa getP() + */ + vpMatrix getLargeP() const { return P; } + + /*! + * Return the task jacobian \f$J\f$. The task jacobian is updated after a call + * of computeControlLaw(). + * + * In the general case, the task jacobian is given by \f${\bf J} = {\widehat + * {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$. + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing vpMatrix + * J = task.getTaskJacobian(); // Get the task jacobian used to compute v + * \endcode + * \sa getTaskJacobianPseudoInverse(), getInteractionMatrix() + */ + vpMatrix getTaskJacobian() const { return J1; } - vpMatrix getTaskJacobian() const; - vpMatrix getTaskJacobianPseudoInverse() const; - unsigned int getTaskRank() const; + /*! + * Return the pseudo inverse of the task jacobian \f$J\f$. + * + * In the general case, the task jacobian is given by \f${\bf J} = {\widehat + * {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$. + * + * The task jacobian and its pseudo inverse are updated after a call of computeControlLaw(). + * + * \return Pseudo inverse \f${J}^{+}\f$ of the task jacobian. + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing + * vpMatrix Jp = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v + * \endcode + * + * \sa getTaskJacobian() + */ + vpMatrix getTaskJacobianPseudoInverse() const { return J1p; } /*! - Get task singular values. + * Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw(). + * + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing + * unsigned int rank = task.getTaskRank(); // Get the rank of the task jacobian + * \endcode + */ + unsigned int getTaskRank() const { return rankJ1; } - \return Singular values that relies on the task jacobian pseudo inverse. - */ + /*! + * Get task singular values. + * + * \return Singular values that relies on the task jacobian pseudo inverse. + */ inline vpColVector getTaskSingularValues() const { return sv; } - vpMatrix getWpW() const; + /*! + * Return the projection operator \f${\bf W}^+{\bf W}\f$. This operator is + * updated after a call of computeControlLaw(). + * + * When the dimension of the task is equal to the number of degrees of freedom + * available \f${\bf W^+W = I}\f$. + * + * \code + * vpServo task; + * ... + * vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing + * vpMatrix WpW = task.getWpW(); // Get the projection operator + * \endcode + * \sa getI_WpW() + */ + vpMatrix getWpW() const { return WpW; } /*! - Return the velocity twist matrix used to transform a velocity skew vector - from end-effector frame into the camera frame. - */ + * Return the velocity twist matrix used to transform a velocity skew vector + * from end-effector frame into the camera frame. + */ vpVelocityTwistMatrix get_cVe() const { return cVe; } /*! - Return the velocity twist matrix used to transform a velocity skew vector - from robot fixed frame (also called world or base frame) into the camera - frame. - */ + * Return the velocity twist matrix used to transform a velocity skew vector + * from robot fixed frame (also called world or base frame) into the camera + * frame. + */ vpVelocityTwistMatrix get_cVf() const { return cVf; } + /*! - Return the velocity twist matrix used to transform a velocity skew vector - from robot end-effector frame into the fixed frame (also called world or - base frame). - */ + * Return the velocity twist matrix used to transform a velocity skew vector + * from robot end-effector frame into the fixed frame (also called world or + * base frame). + */ vpVelocityTwistMatrix get_fVe() const { return fVe; } + /*! - Return the robot jacobian expressed in the end-effector frame. - */ + * Return the robot jacobian expressed in the end-effector frame. + */ vpMatrix get_eJe() const { return eJe; } + /*! - Return the robot jacobian expressed in the robot fixed frame (also called - world or base frame). - */ + * Return the robot jacobian expressed in the robot fixed frame (also called + * world or base frame). + */ vpMatrix get_fJe() const { return fJe; } - double getPseudoInverseThreshold() const; + /*! + * Return pseudo-inverse threshold used to test the singular values. If + * a singular value is lower than this threshold we consider that the + * matrix is not full rank. + * + * \sa setPseudoInverseThreshold() + */ + double getPseudoInverseThreshold() const { return m_pseudo_inverse_threshold; } - // destruction (memory deallocation if required) + /*! + * Task destruction. Kill the current and desired visual feature lists. + * + * This function is called in the destructor. Since ViSP > 3.3.0 it is no more + * mandatory to call explicitly kill(). + * + * \code + * vpServo task ; + * vpFeatureThetaU s; + * ... + * task.addFeature(s); // Add current ThetaU feature + * + * task.kill(); // This call is no more mandatory since achieved in the destructor + * \endcode + */ void kill(); + /*! + * Prints on \e os stream information about the task: + * + * \param display_level : Indicates which are the task information to print. See + * vpServo::vpServoPrintType for more details. + * + * \param os : Output stream. + */ void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout); - // Add a secondary task. + /*! + * Compute and return the secondary task vector according to the classic + * projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper + * \cite Marchand05b) or the new large projection operator (see equation(24) + * in the paper \cite Marey:2010). + * + * \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the + * derivative of the secondary task \f${\bf e}_2\f$. + * \param useLargeProjectionOperator : if true will be use the large projection + * operator, if false the classic one (default). + * + * \return The secondary task vector. + * + * If the classic projection operator is used ( useLargeProjectionOperator = + * false (default value)) this function return: + * + * \f[ + * ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t} + * \f] + * + * Note that the secondary task vector need than to be added to the primary + * task which can be in the general case written as: \f[ + * -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})} + * \f] + * + * Otherwise if the new large projection operator is used ( + * useLargeProjectionOperator = true ) this function return: + * + * \f[ + * {\bf P}\frac{\partial {\bf e_2}}{\partial t} + * \f] + * + * where + * + * \f[ + * {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ + * \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| + * {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f] + * + * with + * + * \f[ + * {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf + * J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf + * e }{\bf e }^\top{\bf J_{{\bf e }} } \f] + * + * \warning computeControlLaw() must be call prior to this function since it + * updates the projection operators. + * + * The following sample code shows how to use this method to compute a + * secondary task using the classic projection operator: + * \code + * vpColVector v; + * // Velocity applied to the robot vpColVector de2dt; vpServo task; + * ... + * v = task.computeControlLaw(); // Compute the primary task + * v += task.secondaryTask(de2dt) // Compute and add the secondary task using the classical projection operator + * \endcode + * + * The following sample code shows how to use this method to compute a + * secondary task using the large projection operator: + * \code + * vpColVector v; + * // Velocity applied to the robot vpColVector de2dt; vpServo task; + * ... + * v = task.computeControlLaw(); // Compute the primary task + * v += task.secondaryTask(de2dt, true) // Compute and add the secondary task using the large projection operator + * \endcode + * + * \sa computeControlLaw() + */ vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false); - // Add a secondary task. - vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt, - const bool &useLargeProjectionOperator = false); - // Add a secondary task to avoid the joint limit. - vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin, - const vpColVector &jointMax, const double &rho = 0.1, - const double &rho1 = 0.3, const double &lambda_tune = 0.7); - - void setCameraDoF(const vpColVector &dof); /*! - Set a variable which enables to compute the interaction matrix at each - iteration. + * Compute and return the secondary task vector according to the classic + * projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper + * \cite Marchand05b) or the new large projection operator (see equation(24) + * in the paper \cite Marey:2010). + * + * \param e2 : Value of the secondary task \f${\bf e}_2\f$. + * \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the + * derivative of the secondary task \f${\bf e}_2\f$. + * \param useLargeProjectionOperator: if true will be use the large projection + * operator, if false the classic one (default). + * + * \return The secondary task vector. + * + * If the classic projection operator is used ( useLargeProjectionOperator = + * false (default value)) this function return: + * + * \f[ + * -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf + * e_2}}{\partial t} \f] + * + * Note that the secondary task vector need than to be added to the primary + * task which can be in the general case written as: \f[ + * -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})} + * \f] + * + * Otherwise if the new large projection operator is used ( + * useLargeProjectionOperator = true ) this function return: + * + * \f[ + * -\lambda {\bf P} {\bf e_2} + {\bf P}\frac{\partial {\bf e_2}}{\partial t} + * \f] + * + * where + * + * \f[ + * {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ + * \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| + * {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f] + * + * with + * + * \f[ + * {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf + * J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf + * e }{\bf e }^\top{\bf J_{{\bf e }} } \f] + * + * \warning computeControlLaw() must be call prior to this function since it + * updates the projection operators. + * + * The following sample code shows how to use this method to compute a + * secondary task using the classical projection operator: + * \code + * vpColVector v; + * // Velocity applied to the robot vpColVector e2; vpColVector de2dt; vpServo + * task; + * ... + * v = task.computeControlLaw(); // Compute the primary task + * v += task.secondaryTask(e2, de2dt) // Compute and add the secondary task using the classical projection operator + * \endcode + * + * The following sample code shows how to use this method to compute a + * secondary task using the large projection operator: + * \code + * vpColVector v; + * // Velocity applied to the robot vpColVector e2; vpColVector de2dt; vpServo + * task; + * ... + * v = task.computeControlLaw(); // Compute the primary task + * v += task.secondaryTask(e2, de2dt, true) // Compute and add the secondary task using the large projection operator + * \endcode + * + * \sa computeControlLaw() + */ + vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt, + const bool &useLargeProjectionOperator = false); - When the interaction matrix is computed from the desired features \f${\bf - s}^*\f$ which are in general constant, the interaction matrix \f${\widehat - {\bf L}}_{s^*}\f$ is computed just at the first iteration of the servo - loop. Sometimes, when the desired features are time dependent \f${{\bf - s}(t)}^*\f$ or varying, the interaction matrix need to be computed at each - iteration of the servo loop. This method allows to force the computation - of \f${\widehat {\bf L}}\f$ in this particular case. + /*! + * Compute and return the secondary task vector for joint limit avoidance + * \cite Marey:2010b using the new large projection operator (see equation(24) + * in the paper \cite Marey:2010). The robot avoids the joint limits very + * smoothly even when the main task constrains all the robot degrees of freedom. + * + * \param q : Actual joint positions vector + * + * \param dq : Actual joint velocities vector + * + * \param qmin : Vector containing the low limit value of each joint in the chain. + * \param qmax : Vector containing the high limit value of each joint in the chain. + * + * \param rho : tuning parameter \f${\left [ 0,\frac{1}{2} \right]}\f$ + * used to define the safe configuration for the joint. When the joint + * angle value cross the max or min boundaries (\f${ q_{l_{0}}^{max} }\f$ and + * \f${q_{l_{0}}^{min}}\f$) the secondary task is activated gradually. + * + * \param rho1 : tuning parameter \f${\left ] 0,1 \right ]}\f$ to compute the external + * boundaries (\f${q_{l_{1}}^{max}}\f$ and \f${q_{l_{1}}^{min}}\f$) for the joint + * limits. Here the secondary task it completely activated with the highest gain. + * + * \param lambda_tune : value \f${\left [ 0,1 \right ]}\f$ used to tune the + * difference in magnitude between the absolute value of the elements of the + * primary task and the elements of the secondary task. (See equation (17) + * \cite Marey:2010b ) + * + * \code + * vpServo task; + * vpColVector qmin; + * vpColVector qmax; + * vpColVector q; + * vpColVector dq; + * // Fill vector qmin and qmax with min and max limits of the joints (same joint order than vector q). + * // Update vector of joint position q and velocities dq; + * ... + * // Compute the velocity corresponding to the visual servoing + * vpColVector v = task.computeControlLaw(); + * // Compute and add the secondary task for the joint limit avoidance + * // using the large projection operator + * v += task.secondaryTaskJointLimitAvoidance(q, dq, qmin, qmax) + * \endcode + */ + vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin, + const vpColVector &qmax, const double &rho = 0.1, + const double &rho1 = 0.3, const double &lambda_tune = 0.7); - \param force_computation : If true it forces the interaction matrix - computation even if it is already done. + /*! + * Set a 6-dim column vector representing the degrees of freedom that are + * controlled in the camera frame. When set to 1, all the 6 dof are controlled. + * + * \param dof : Degrees of freedom to control in the camera frame. + * Below we give the correspondence between the index of the vector and the + * considered dof: + * - dof[0] = 1 if translation along X is controled, 0 otherwise; + * - dof[1] = 1 if translation along Y is controled, 0 otherwise; + * - dof[2] = 1 if translation along Z is controled, 0 otherwise; + * - dof[3] = 1 if rotation along X is controled, 0 otherwise; + * - dof[4] = 1 if rotation along Y is controled, 0 otherwise; + * - dof[5] = 1 if rotation along Z is controled, 0 otherwise; + * + * The following example shows how to use this function to control only wx, wy + * like a pan/tilt: + * \code + * #include + * #include + * + * int main() + * { + * vpServo servo; + * servo.setServo(vpServo::EYEINHAND_CAMERA); + * vpFeaturePoint s, sd; + * servo.addFeature(s, sd); + * + * vpColVector dof(6, 1); + * dof[0] = 0; // turn off vx + * dof[1] = 0; // turn off vy + * dof[2] = 0; // turn off vz + * dof[5] = 0; // turn off wz + * servo.setCameraDoF(dof); + * + * while(1) { + * // vpFeatureBuilder::create(s, ...); // update current feature + * + * vpColVector v = servo.computeControlLaw(); // compute control law + * // only v[3] and v[4] corresponding to wx and wy are different from 0 + * } + * } + * \endcode + */ + void setCameraDoF(const vpColVector &dof); + /*! + * Set a variable which enables to compute the interaction matrix at each + * iteration. + * + * When the interaction matrix is computed from the desired features \f${\bf + * s}^*\f$ which are in general constant, the interaction matrix \f${\widehat + * {\bf L}}_{s^*}\f$ is computed just at the first iteration of the servo + * loop. Sometimes, when the desired features are time dependent \f${{\bf + * s}(t)}^*\f$ or varying, the interaction matrix need to be computed at each + * iteration of the servo loop. This method allows to force the computation + * of \f${\widehat {\bf L}}\f$ in this particular case. + * + * \param force_computation : If true it forces the interaction matrix + * computation even if it is already done. */ void setForceInteractionMatrixComputation(bool force_computation) { @@ -379,121 +952,133 @@ class VISP_EXPORT vpServo } /*! - Set the interaction matrix type (current, desired, mean or user defined) - and how its inverse is computed. - - \param interactionMatrixType : The interaction matrix type. See vpServo::vpServoIteractionMatrixType for more - details. - - \param interactionMatrixInversion : How is the inverse computed. See vpServo::vpServoInversionType for more details. - */ + * Set the interaction matrix type (current, desired, mean or user defined) + * and how its inverse is computed. + * + * \param interactionMatrixType : The interaction matrix type. See vpServo::vpServoIteractionMatrixType for more + * details. + * + * \param interactionMatrixInversion : How is the inverse computed. See vpServo::vpServoInversionType for more details. + */ void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, - const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE); + const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE); /*! - Set the gain \f$\lambda\f$ used in the control law (see - vpServo::vpServoType) as constant. - - The usage of an adaptive gain allows to reduce the convergence time, see - setLambda(const vpAdaptiveGain&). - - \param c : Constant gain. Values are in general between 0.1 and 1. Higher - is the gain, higher are the velocities that may be applied to the robot. - */ + * Set the gain \f$\lambda\f$ used in the control law (see + * vpServo::vpServoType) as constant. + * + * The usage of an adaptive gain allows to reduce the convergence time, see + * setLambda(const vpAdaptiveGain&). + * + * \param c : Constant gain. Values are in general between 0.1 and 1. Higher + * is the gain, higher are the velocities that may be applied to the robot. + */ void setLambda(double c) { lambda.initFromConstant(c); } /*! - Set the gain \f$\lambda\f$ used in the control law (see - vpServo::vpServoType) as adaptive. Value of \f$\lambda\f$ that is used in - computeControlLaw() depend on the infinity norm of the task Jacobian. - - The usage of an adaptive gain rather than a constant gain allows to reduce - the convergence time. - - \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$. - \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$. - \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$. - - For more details on these parameters see vpAdaptiveGain class. - */ + * Set the gain \f$\lambda\f$ used in the control law (see + * vpServo::vpServoType) as adaptive. Value of \f$\lambda\f$ that is used in + * computeControlLaw() depend on the infinity norm of the task Jacobian. + * + * The usage of an adaptive gain rather than a constant gain allows to reduce + * the convergence time. + * + * \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$. + * \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$. + * \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$. + * + * For more details on these parameters see vpAdaptiveGain class. + */ void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero) { lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero); } - /*! - Set the gain \f$\lambda\f$ used in the control law (see - vpServo::vpServoType) as adaptive. Value of \f$\lambda\f$ that is used in - computeControlLaw() depend on the infinity norm of the task Jacobian. - The usage of an adaptive gain rather than a constant gain allows to reduce - the convergence time. - - \sa vpAdaptiveGain - */ - void setLambda(const vpAdaptiveGain &l) { lambda = l; } /*! - Set the value of the parameter \f$\mu\f$ used to ensure the continuity of - the velocities computed using computeControlLaw(double). + * Set the gain \f$\lambda\f$ used in the control law (see + * vpServo::vpServoType) as adaptive. Value of \f$\lambda\f$ that is used in + * computeControlLaw() depend on the infinity norm of the task Jacobian. + * + * The usage of an adaptive gain rather than a constant gain allows to reduce + * the convergence time. + * + * \sa vpAdaptiveGain + */ + void setLambda(const vpAdaptiveGain &l) { lambda = l; } - A recommended value is 4. - */ + /*! + * Set the value of the parameter \f$\mu\f$ used to ensure the continuity of + * the velocities computed using computeControlLaw(double). + * + * A recommended value is 4. + */ void setMu(double mu_) { this->mu = mu_; } - // Choice of the visual servoing control law + + /*! + * Set the visual servoing control law. + * \param servo_type : Control law that will be considered. + * See vpServo::vpServoType to see the possible values. + */ void setServo(const vpServoType &servo_type); /*! - Set the velocity twist matrix used to transform a velocity skew vector - from end-effector frame into the camera frame. - */ + * Set the velocity twist matrix used to transform a velocity skew vector + * from end-effector frame into the camera frame. + */ void set_cVe(const vpVelocityTwistMatrix &cVe_) { this->cVe = cVe_; init_cVe = true; } + /*! - Set the velocity twist matrix used to transform a velocity skew vector - from end-effector frame into the camera frame. - */ + * Set the velocity twist matrix used to transform a velocity skew vector + * from end-effector frame into the camera frame. + */ void set_cVe(const vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe = true; } + /*! - Set the velocity twist matrix used to transform a velocity skew vector - from robot fixed frame (also called world or base frame) into the camera - frame. - */ + * Set the velocity twist matrix used to transform a velocity skew vector + * from robot fixed frame (also called world or base frame) into the camera + * frame. + */ void set_cVf(const vpVelocityTwistMatrix &cVf_) { this->cVf = cVf_; init_cVf = true; } + /*! - Set the velocity twist matrix used to transform a velocity skew vector - from robot fixed frame (also called world or base frame) into the camera - frame. - */ + * Set the velocity twist matrix used to transform a velocity skew vector + * from robot fixed frame (also called world or base frame) into the camera + * frame. + */ void set_cVf(const vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf = true; } + /*! - Set the velocity twist matrix used to transform a velocity skew vector - from robot end-effector frame into the fixed frame (also called world or - base frame). - */ + * Set the velocity twist matrix used to transform a velocity skew vector + * from robot end-effector frame into the fixed frame (also called world or + * base frame). + */ void set_fVe(const vpVelocityTwistMatrix &fVe_) { this->fVe = fVe_; init_fVe = true; } + /*! - Set the velocity twist matrix used to transform a velocity skew vector - from robot end-effector frame into the fixed frame (also called world or - base frame). - */ + * Set the velocity twist matrix used to transform a velocity skew vector + * from robot end-effector frame into the fixed frame (also called world or + * base frame). + */ void set_fVe(const vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); @@ -501,44 +1086,68 @@ class VISP_EXPORT vpServo } /*! - Set the robot jacobian expressed in the end-effector frame. - */ + * Set the robot jacobian expressed in the end-effector frame. + */ void set_eJe(const vpMatrix &eJe_) { this->eJe = eJe_; init_eJe = true; } + /*! - Set the robot jacobian expressed in the robot fixed frame (also called - world or base frame). - */ + * Set the robot jacobian expressed in the robot fixed frame (also called + * world or base frame). + */ void set_fJe(const vpMatrix &fJe_) { this->fJe = fJe_; init_fJe = true; } - void setPseudoInverseThreshold(double pseudo_inverse_threshold); + /*! + * Set the pseudo-inverse threshold used to test the singular values. If + * a singular value is lower than this threshold we consider that the + * matrix is not full rank. + * \param pseudo_inverse_threshold : Value to use. Default value is set to 1e-6. + * \sa getPseudoInverseThreshold() + */ + void setPseudoInverseThreshold(double pseudo_inverse_threshold) + { + m_pseudo_inverse_threshold = pseudo_inverse_threshold; + } /*! - Test if all the initialization are correct. If true, the control law can - be computed. - */ + * Test if all the initialization are correct. If true, the control law can + * be computed. + */ bool testInitialization(); + /*! - Test if all the update are correct. If true control law can be computed. - */ + * Test if all the update are correct. If true control law can be computed. + */ bool testUpdated(); protected: - //! Basic initialization. + /*! + * Initialize the servo with the following settings: + * + * - No control law is specified. The user has to call setServo() to specify + * the control law. + * - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is + * computed with the desired features \f${\bf s}^*\f$. Using + * setInteractionMatrixType() you can also compute the interaction matrix with + * the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s + * + {\widehat {\bf L}}_{s^*}\right)/2\f$. + * - In the control law the pseudo inverse will be used. The method + * setInteractionMatrixType() allows to use the transpose instead. + */ void init(); /*! - Compute the classic projection operator and the large projection operator. - */ + * Compute the classic projection operator and the large projection operator. + */ void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, - const vpColVector &error_, vpMatrix &P_) const; + const vpColVector &error_, vpMatrix &P_) const; public: //! Interaction matrix @@ -603,30 +1212,61 @@ class VISP_EXPORT vpServo Twist transformation matrix */ - //! Twist transformation matrix between Re and Rc. + /*! + * Twist transformation matrix between camera frame (c) and robot end-effector + * frame (e). + */ vpVelocityTwistMatrix cVe; + + /*! + * Boolean indicating if twist transformation matrix between camera frame (c) + * and robot end-effector frame (e) is set by the user and thus differs from eye + * matrix. + */ bool init_cVe; - //! Twist transformation matrix between Rf and Rc. + //! Twist transformation matrix between camera frame (c) and robot base frame (f). vpVelocityTwistMatrix cVf; + /*! + * Boolean indicating if twist transformation matrix between camera frame (c) + * and robot base frame (f) is set by the user and thus differs from eye + * matrix. + */ bool init_cVf; - //! Twist transformation matrix between Re and Rf. + /*! + * Twist transformation matrix between robot base frame (f) and robot + * end-effector frame (e). + */ vpVelocityTwistMatrix fVe; + /*! + * Boolean indicating if twist transformation matrix between robot base frame (f) + * and robot end-effector frame(e) is set by the user and thus differs from eye + * matrix. + */ bool init_fVe; /* - Jacobians - */ + * Jacobians + */ - //! Jacobian expressed in the end-effector frame. + //! Jacobian expressed in the end-effector frame (e). vpMatrix eJe; + /*! + * Boolean indicating if Jacobian expressed in the end-effector frame (e) + * is set by the user and thus differs from eye matrix. + */ bool init_eJe; - //! Jacobian expressed in the robot reference frame. + + //! Jacobian expressed in the robot base frame (f). vpMatrix fJe; + /*! + * Boolean indicating if Jacobian expressed in the robot base frame (f) + * is set by the user and thus differs from eye matrix. + */ bool init_fJe; /* - Task building - */ + * Task building + */ //! true if the error has been computed. bool errorComputed; @@ -646,36 +1286,47 @@ class VISP_EXPORT vpServo //! Projection operators \f$\bf I-WpW\f$. vpMatrix I_WpW; /*! - New Large projection operator (see equation(24) in the paper - \cite Marey:2010). This projection operator allows performing secondary task - even when the main task is full rank. \f[ - {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ - \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| - {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f] - - with - - \f[ - {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf - J_{{\bf e }} } {\bf J_{{\bf e }}^\top } - {\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} } - \f] - + * New Large projection operator (see equation(24) in the paper + * \cite Marey:2010). This projection operator allows performing secondary task + * even when the main task is full rank. \f[ + * {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ + * \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| + * {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f] + * + * with + * + * \f[ + * {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf + * J_{{\bf e }} } {\bf J_{{\bf e }}^\top } + * {\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} } + * \f] */ vpMatrix P; //! Singular values from the pseudo inverse. vpColVector sv; + /*! + * Gain \f$ mu \f$ used to compute the control law with the task + * sequencing approach. + * \see computeControlLaw(double) and computeControlLaw(double, const vpColVector &) + */ double mu; + /*! + * First primary task value \f${\bf \widehat J}_e^+ {\bf e}\f$ used in the control + * law with the task sequencing approach when time is initial \f$t=0\f$. + * \see computeControlLaw(double) and computeControlLaw(double, const vpColVector &) + */ vpColVector e1_initial; //! Boolean to know if cJc is identity (for fast computation) bool iscJcIdentity; - //! A diag matrix used to determine which are the degrees of freedom that - //! are controlled in the camera frame + /*! + * A diag matrix used to determine which are the degrees of freedom that + * are controlled in the camera frame (c). + */ vpMatrix cJc; bool m_first_iteration; //!< True until first call of computeControlLaw() is achieved diff --git a/modules/vs/include/visp3/vs/vpServoData.h b/modules/vs/include/visp3/vs/vpServoData.h index b67f2bb1bd..cea85db513 100644 --- a/modules/vs/include/visp3/vs/vpServoData.h +++ b/modules/vs/include/visp3/vs/vpServoData.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,10 @@ * * Description: * Save data during the task execution. - * -*****************************************************************************/ + */ -#ifndef vpServoData_H -#define vpServoData_H +#ifndef _vpServoData_h_ +#define _vpServoData_h_ /*! \file vpServoData.h @@ -47,10 +45,10 @@ #include /*! - \class vpServoData - \ingroup group_task - \brief Save data during the task execution. -*/ + * \class vpServoData + * \ingroup group_task + * \brief Save data during the task execution when using vpServo. + */ class VISP_EXPORT vpServoData { private: @@ -78,18 +76,44 @@ class VISP_EXPORT vpServoData } #endif + /*! + * Default constructor. + */ vpServoData() : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), vNormFile(), cmDeg(false) { ; } - virtual ~vpServoData() { ; } - //! velocity output in cm and deg + /*! + * Destructor that closes all data files if needed. + */ + virtual ~vpServoData() { close(); } + + //! Velocity output are set in cm and deg. void setCmDeg(); - //! velocity output in meter and deg (default) + //! Velocity output are set in meter and deg (default). void setMeterRad(); - + //! Save visual-servoing control law data. void save(const vpServo &task); + + /*! + * Set the directory in which data are saved. + * In this directory, creates the following files: + * - `vel.dat` that contains velocities computed + * - `error.dat` that contains visual-servo error \f$ {\bf e} = ({\bf s} - {\bf s}^*)\f$ + * - `errornorm.dat` that contains the sum square of the visual-servo error \f$ {\bf e} \f$ + * - `s.dat` that contains the current feature vector \f$ \bf s \f$ + * - `sStar.dat` that contains the desired feature vector \f$ {\bf s}^* \f$ + * + * @param directory : Path to the folder that contains data files to save. + * + * \sa close() + */ void open(const std::string &directory); - void close(); + /*! + * Close all data files open with open() function. + * + * \sa open() + */ + void close(); }; #endif diff --git a/modules/vs/include/visp3/vs/vpServoDisplay.h b/modules/vs/include/visp3/vs/vpServoDisplay.h index fa1743682f..0da40d004f 100644 --- a/modules/vs/include/visp3/vs/vpServoDisplay.h +++ b/modules/vs/include/visp3/vs/vpServoDisplay.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,38 +29,70 @@ * * Description: * Interface with the image for feature display. - * -*****************************************************************************/ + */ #ifndef vpServoDisplay_H #define vpServoDisplay_H /*! - \file vpServoDisplay.h - \brief interface with the image for feature display -*/ + * \file vpServoDisplay.h + * \brief interface with the image for feature display + */ -// Servo #include - -// Meter/pixel conversion #include - -// Color / image / display #include #include #include + /*! - \class vpServoDisplay - \ingroup group_task - \brief Interface with the image for feature display. -*/ + * \class vpServoDisplay + * \ingroup group_task + * \brief Interface with the image for feature display. + */ class VISP_EXPORT vpServoDisplay { public: + /*! + * Display the current and the desired features in the image I. + * + * \warning To effectively display the dot graphics a call to + * vpDisplay::flush() is needed. + * + * \param s : Visual servoing control law. + * \param cam : Camera parameters. + * \param I : Image on which features have to be displayed. + * + * \param currentColor : Color for the current features. If vpColor::none, + * current features display is turned off. + * + * \param desiredColor : Color for the desired features. If vpColor::none, + * desired features display is turned off. + * + * \param thickness : Thickness of the feature representation. + */ static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, vpColor currentColor = vpColor::green, vpColor desiredColor = vpColor::red, unsigned int thickness = 1); + + /*! + * Display the current and the desired features in the image I. + * + * \warning To effectively display the dot graphics a call to + * vpDisplay::flush() is needed. + * + * \param s : Visual servoing control law. + * \param cam : Camera parameters. + * \param I : Color image on which features have to be displayed. + * + * \param currentColor : Color for the current features. If vpColor::none, + * current features display is turned off. + * + * \param desiredColor : Color for the desired features. If vpColor::none, + * desired features display is turned off. + * + * \param thickness : Thickness of the feature representation. + */ static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, vpColor currentColor = vpColor::green, vpColor desiredColor = vpColor::red, unsigned int thickness = 1); diff --git a/modules/vs/include/visp3/vs/vpServoException.h b/modules/vs/include/visp3/vs/vpServoException.h index 2615f2ab26..2b2a65b954 100644 --- a/modules/vs/include/visp3/vs/vpServoException.h +++ b/modules/vs/include/visp3/vs/vpServoException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Exception that can be emitted by the vpServo class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpServoException_h_ #define _vpServoException_h_ @@ -59,7 +57,7 @@ class VISP_EXPORT vpServoException : public vpException */ enum errorServoCodeEnum { -//! Current or desired feature list is empty + //! Current or desired feature list is empty noFeatureError, //! No degree of freedom is available to achieve the secondary task. noDofFree, diff --git a/modules/vs/src/vpAdaptiveGain.cpp b/modules/vs/src/vpAdaptiveGain.cpp index 7d16f36dec..72c373c572 100644 --- a/modules/vs/src/vpAdaptiveGain.cpp +++ b/modules/vs/src/vpAdaptiveGain.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,13 +29,10 @@ * * Description: * Adaptive gain. - * - * Authors: - * Nicolas Mansard - * -*****************************************************************************/ + */ + /*! -\file vpAdaptiveGain.cpp + \file vpAdaptiveGain.cpp */ /* --- VISP --- */ @@ -52,22 +48,6 @@ const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666; const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666; const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666; -/* -------------------------------------------------------------------------- - */ -/* --- CONSTRUCTION --------------------------------------------------------- - */ -/* -------------------------------------------------------------------------- - */ - -/*! - Basic constructor which initializes all the parameters with their default - value: - - \f$ \lambda_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO - - \f$ \lambda_\infty = 0.1666 \f$ using - vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY - - \f$ \lambda'_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE - -*/ vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { this->initFromVoid(); @@ -75,43 +55,14 @@ vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.) return; } -/*! - Constructor that initializes the gain as constant. In that case - \f$\lambda(||{\bf e}||) = c\f$. - - \param c : Value of the constant gain. A typical value is 0.5. -*/ vpAdaptiveGain::vpAdaptiveGain(double c) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initFromConstant(c); } -/*! - Constructor that initializes the gain as adaptive. - - \param gain_at_zero : the expected gain when \f$||{\bf e}||=0\f$: - \f$\lambda_0\f$. \param gain_at_infinity : the expected gain when \f$||{\bf - e}||\rightarrow\infty\f$: \f$\lambda_\infty\f$. \param slope_at_zero : the - expected slope of \f$\lambda(||{\bf e}||)\f$ when \f$||{\bf e}||=0\f$: - \f$\lambda'_0\f$. - -*/ vpAdaptiveGain::vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initStandard(gain_at_zero, gain_at_infinity, slope_at_zero); } -/* -------------------------------------------------------------------------- - */ -/* --- INIT ----------------------------------------------------------------- - */ -/* -------------------------------------------------------------------------- - */ - -/*! - Initializes the parameters to have a constant gain. In that case - \f$\lambda(||{\bf e}||) = c\f$. - - \param c : Value of the constant gain. A typical value is 0.5. -*/ void vpAdaptiveGain::initFromConstant(const double c) { this->coeff_a = 0; @@ -120,13 +71,6 @@ void vpAdaptiveGain::initFromConstant(const double c) return; } -/*! - Initializes the parameters with the default value : - - \f$ \lambda_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO - - \f$ \lambda_\infty = 0.1666 \f$ using - vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY - - \f$ \lambda'_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE -*/ void vpAdaptiveGain::initFromVoid(void) { this->initStandard(vpAdaptiveGain::DEFAULT_LAMBDA_ZERO, vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY, @@ -134,23 +78,14 @@ void vpAdaptiveGain::initFromVoid(void) return; } -/*! - Set the parameters \f$\lambda_0, \lambda_\infty, \lambda'_0\f$ used to - compute \f$\lambda(||{\bf e}||)\f$. - - \param gain_at_zero : the expected gain when \f$||{\bf e}||=0\f$: - \f$\lambda_0\f$. \param gain_at_infinity : the expected gain when \f$||{\bf - e}||\rightarrow\infty\f$: \f$\lambda_\infty\f$. \param slope_at_zero : the - expected slope of \f$\lambda(||{\bf e}||)\f$ when \f$||{\bf e}||=0\f$: - \f$\lambda'_0\f$. -*/ void vpAdaptiveGain::initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero) { this->coeff_a = gain_at_zero - gain_at_infinity; // if (0 == this ->coeff_a) if (std::fabs(this->coeff_a) <= std::numeric_limits::epsilon()) { this->coeff_b = 0; - } else { + } + else { this->coeff_b = slope_at_zero / (this->coeff_a); } this->coeff_c = gain_at_infinity; @@ -158,19 +93,6 @@ void vpAdaptiveGain::initStandard(double gain_at_zero, double gain_at_infinity, return; } -/* -------------------------------------------------------------------------- - */ -/* --- MODIFICATOR ---------------------------------------------------------- - */ -/* -------------------------------------------------------------------------- - */ - -/*! - Sets the internal parameters in order to obtain a constant gain equal to - the gain in 0 set through the parameter \f$\lambda_0\f$. - - \return It returns the value of the constant gain \f$\lambda_0\f$. -*/ double vpAdaptiveGain::setConstant(void) { double res = this->coeff_a + this->coeff_c; @@ -182,25 +104,6 @@ double vpAdaptiveGain::setConstant(void) return res; } -/* -------------------------------------------------------------------------- - */ -/* --- VALEUR --------------------------------------------------------------- - */ -/* -------------------------------------------------------------------------- - */ - -/*! - Computes the value of the adaptive gain \f$\lambda(x)\f$ using: - - \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ - \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f] - - \param x : Input value to consider. During a visual servo this value can be - the Euclidean norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf - e}||_{\infty}\f$ of the task function. - - \return It returns the value of the computed gain. -*/ double vpAdaptiveGain::value_const(double x) const { double res = this->coeff_a * exp(-this->coeff_b * x) + this->coeff_c; @@ -208,13 +111,6 @@ double vpAdaptiveGain::value_const(double x) const return res; } -/*! - Gets the value of the gain at infinity (ie the value of \f$ \lambda_\infty = - c \f$). This function is similar to limitValue() except that here the value - is not stored as a parameter of the class. - - \return It returns the value of the gain at infinity. - */ double vpAdaptiveGain::limitValue_const(void) const { double res = this->coeff_c; @@ -222,20 +118,6 @@ double vpAdaptiveGain::limitValue_const(void) const return res; } -/*! - Computes the value of the adaptive gain \f$\lambda(x)\f$ using: - - \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ - \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f] - - This value is stored as a parameter of the class. - - \param x : Input value to consider. During a visual servo this value can be - the Euclidean norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf - e}||_{\infty}\f$ of the task function. - - \return It returns the value of the computed gain. - */ double vpAdaptiveGain::value(double x) const { this->lambda = this->value_const(x); @@ -243,12 +125,6 @@ double vpAdaptiveGain::value(double x) const return lambda; } -/*! - Gets the value of the gain at infinity (ie the value of \f$\lambda_\infty = - c \f$) and stores it as a parameter of the class. - - \return It returns the value of the gain at infinity. - */ double vpAdaptiveGain::limitValue(void) const { this->lambda = this->limitValue_const(); @@ -256,70 +132,16 @@ double vpAdaptiveGain::limitValue(void) const return lambda; } -/* -------------------------------------------------------------------------- - */ -/* --- ACCESSORS ------------------------------------------------------------ - */ -/* -------------------------------------------------------------------------- - */ - -/*! - Operator that computes \f$\lambda(x)\f$ where - - \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ - \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f] - - \param x : Input value to consider. During a visual servo this value can be - the Euclidean norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf - e}||_{\infty}\f$ of the task function. - - \return It returns the value of the computed gain. - - \sa value() -*/ double vpAdaptiveGain::operator()(double x) const { return this->value(x); } -/*! - Gets the value of the gain at infinity (ie the value of \f$\lambda_\infty = - c \f$). - - \return It returns the value of the gain at infinity. - - \sa limitValue() - */ double vpAdaptiveGain::operator()(void) const { return this->limitValue(); } -/*! - Operator which computes \f$\lambda({||x||}_{\infty})\f$ where - - \f[ \lambda ({||x||}_{\infty}) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ - \lambda'_0}{\lambda_0 - \lambda_\infty}{||x||}_{\infty}} + \lambda_\infty - \f] - - \param x : Input vector \f$ \bf x\f$ to consider. - - \return It returns the value of the computed gain. -*/ double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); } -/* -------------------------------------------------------------------------- - */ -/* --- OUTPUT --------------------------------------------------------------- - */ -/* -------------------------------------------------------------------------- - */ - -/*! - Prints the adaptive gain parameters \f$\lambda_0, \lambda_\infty, - \lambda'_0\f$. - - \param os : The stream where to print the adaptive gain parameters. - \param lambda : The adaptive gain containing the parameters to print. -*/ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda) { os << "Zero= " << lambda.coeff_a + lambda.coeff_c << "\tInf= " << lambda.coeff_c - << "\tSlope= " << lambda.coeff_a * lambda.coeff_b; + << "\tSlope= " << lambda.coeff_a * lambda.coeff_b; return os; } diff --git a/modules/vs/src/vpServo.cpp b/modules/vs/src/vpServo.cpp index 442539eb91..50e67df55a 100644 --- a/modules/vs/src/vpServo.cpp +++ b/modules/vs/src/vpServo.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,40 +29,19 @@ * * Description: * Visual servoing control law. - * -*****************************************************************************/ - -#include + */ #include -// Exception #include - -// Debug trace #include +#include /*! \file vpServo.cpp \brief Class required to compute the visual servoing control law */ -/*! - Default constructor that initializes the following settings: - - No control law is specified. The user has to call setServo() to specify - the control law. - - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is - computed with the desired features \f${\bf s}^*\f$. Using - setInteractionMatrixType() you can also compute the interaction matrix with - the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s - + {\widehat {\bf L}}_{s^*}\right)/2\f$. - - In the control law the pseudo inverse will be used. The method - setInteractionMatrixType() allows to use the transpose instead. - - \warning By default the threshold used to compute the pseudo-inverse is set to 1e-6. - Advanced user can modify this value using setPseudoInverseThreshold(). - -*/ vpServo::vpServo() : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE), rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1), @@ -76,20 +54,6 @@ vpServo::vpServo() cJc.eye(); } -/*! - Constructor that allows to choose the visual servoing control law. - \param servo_type : Visual servoing control law. - - The other settings are the following: - - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is - computed with the desired features \f${\bf s}^*\f$. Using - setInteractionMatrixType() you can also compute the interaction matrix with - the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s - + {\widehat {\bf L}}_{s^*}\right)/2\f$. - - In the control law the pseudo inverse will be used. The method - setInteractionMatrixType() allows to use the transpose instead. - - */ vpServo::vpServo(vpServoType servo_type) : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type), rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1), interactionMatrixType(DESIRED), @@ -101,29 +65,8 @@ vpServo::vpServo(vpServoType servo_type) cJc.eye(); } -/*! - Destructor. - - Since ViSP > 3.3.0 calls kill() to destroy the current and desired feature lists. - - \sa kill() -*/ vpServo::~vpServo() { kill(); } -/*! - Initialize the servo with the following settings: - - - No control law is specified. The user has to call setServo() to specify - the control law. - - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is - computed with the desired features \f${\bf s}^*\f$. Using - setInteractionMatrixType() you can also compute the interaction matrix with - the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s - + {\widehat {\bf L}}_{s^*}\right)/2\f$. - - In the control law the pseudo inverse will be used. The method - setInteractionMatrixType() allows to use the transpose instead. - -*/ void vpServo::init() { // type of visual servoing @@ -160,22 +103,6 @@ void vpServo::init() m_first_iteration = true; } -/*! - Task destruction. Kill the current and desired visual feature lists. - - This function is called in the destructor. Since ViSP > 3.3.0 it is no more - mandatory to call explicitly kill(). - - \code - vpServo task ; - vpFeatureThetaU s; - ... - task.addFeature(s); // Add current ThetaU feature - - task.kill(); // This call is no more mandatory since achieved in the destructor - \endcode - -*/ void vpServo::kill() { if (taskWasKilled == false) { @@ -202,11 +129,6 @@ void vpServo::kill() } } -/*! - Set the visual servoing control law. - \param servo_type : Control law that will be considered. - See vpServo::vpServoType to see the possible values. - */ void vpServo::setServo(const vpServoType &servo_type) { this->servoType = servo_type; @@ -228,49 +150,6 @@ void vpServo::setServo(const vpServoType &servo_type) }; } -/*! - Set a 6-dim column vector representing the degrees of freedom that are - controlled in the camera frame. When set to 1, all the 6 dof are controlled. - - \param dof : Degrees of freedom to control in the camera frame. - Below we give the correspondance between the index of the vector and the - considered dof: - - dof[0] = 1 if translation along X is controled, 0 otherwise; - - dof[1] = 1 if translation along Y is controled, 0 otherwise; - - dof[2] = 1 if translation along Z is controled, 0 otherwise; - - dof[3] = 1 if rotation along X is controled, 0 otherwise; - - dof[4] = 1 if rotation along Y is controled, 0 otherwise; - - dof[5] = 1 if rotation along Z is controled, 0 otherwise; - - The following example shows how to use this function to control only wx, wy - like a pan/tilt: - \code -#include -#include - -int main() -{ - vpServo servo; - servo.setServo(vpServo::EYEINHAND_CAMERA); - vpFeaturePoint s, sd; - servo.addFeature(s, sd); - - vpColVector dof(6, 1); - dof[0] = 0; // turn off vx - dof[1] = 0; // turn off vy - dof[2] = 0; // turn off vz - dof[5] = 0; // turn off wz - servo.setCameraDoF(dof); - - while(1) { - // vpFeatureBuilder::create(s, ...); // update current feature - - vpColVector v = servo.computeControlLaw(); // compute control law - // only v[3] and v[4] corresponding to wx and wy are different from 0 - } -} - \endcode -*/ void vpServo::setCameraDoF(const vpColVector &dof) { if (dof.size() == 6) { @@ -287,15 +166,6 @@ void vpServo::setCameraDoF(const vpColVector &dof) } } -/*! - - Prints on \e os stream information about the task: - - \param displayLevel : Indicates which are the task information to print. See - vpServo::vpServoPrintType for more details. - - \param os : Output stream. -*/ void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os) { switch (displayLevel) { @@ -456,34 +326,6 @@ void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream & } } -/*! - Add a new set of 2 features \f$\bf s\f$ and \f${\bf s}^*\f$ in the task. - - \param s_cur : Current visual feature denoted \f$\bf s\f$. - \param s_star : Desired visual feature denoted \f${\bf s}^*\f$. - \param select : Feature selector. By default all the features in \e s and \e - s_star are used, but is is possible to specify which one is used in case of - multiple features. - - The following sample code explain how to use this method to add a visual - feature point \f$(x,y)\f$: - \code - vpFeaturePoint s, s_star; - ... - vpServo task; - task.addFeature(s, s_star); - \endcode - - For example to use only the \f$x\f$ visual feature, the previous code - becomes: - \code - vpFeaturePoint s, s_star; - ... - vpServo task; - task.addFeature(s, s_star, vpFeaturePoint::selectX()); - \endcode - - */ void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select) { featureList.push_back(&s_cur); @@ -491,33 +333,6 @@ void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned featureSelectionList.push_back(select); } -/*! - Add a new features \f$\bf s\f$ in the task. The desired visual feature - denoted \f${\bf s}^*\f$ is equal to zero. - - \param s_cur : Current visual feature denoted \f$\bf s\f$. - \param select : Feature selector. By default all the features in \e s are - used, but is is possible to specify which one is used in case of multiple - features. - - The following sample code explain how to use this method to add a \f$\theta - {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature: - \code - vpFeatureThetaU s(vpFeatureThetaU::cRcd); - ... - vpServo task; - task.addFeature(s); - \endcode - - For example to use only the \f$\theta u_x\f$ feature, the previous code - becomes: - \code - vpFeatureThetaU s(vpFeatureThetaU::cRcd); - ... - vpServo task; - task.addFeature(s, vpFeatureThetaU::selectTUx); - \endcode - */ void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select) { featureList.push_back(&s_cur); @@ -546,7 +361,6 @@ void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select) featureSelectionList.push_back(select); } -//! Return the task dimension. unsigned int vpServo::getDimension() const { unsigned int dim = 0; @@ -633,14 +447,6 @@ static void computeInteractionMatrixFromList(const std::list & return; } -/*! - - Compute and return the interaction matrix related to the set of visual - features. - - \return The interaction matrix \f${\widehat {\bf L}}_e\f$ used in the - control law specified using setServo(). -*/ vpMatrix vpServo::computeInteractionMatrix() { try { @@ -698,14 +504,6 @@ vpMatrix vpServo::computeInteractionMatrix() return L; } -/*! - - Compute the error \f$\bf e =(s - s^*)\f$ between the current set of visual - features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$. - - \return The error vector \f$\bf e\f$. - -*/ vpColVector vpServo::computeError() { if (featureList.empty()) { @@ -901,32 +699,7 @@ bool vpServo::testUpdated() return false; } -/*! - Compute the control law specified using setServo(). See vpServo::vpServoType - for more details concerning the control laws that are available. The \ref - tutorial-ibvs and \ref tutorial-boost-vs are also useful to illustrate the - usage of this function. - - The general form of the control law is the following: - - \f[ - {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} - \f] - - where : - - \f${\bf \dot q}\f$ is the resulting velocity command to apply to the - robot. - - the sign of the control law depends on the eye in hand or eye to hand - configuration. - - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction - matrix and of the robot Jacobian. - - \f$\bf e = (s-s^*)\f$ is the error to regulate. - - To ensure continuous sequencing the computeControlLaw(double) function can - be used. It will ensure that the velocities that are computed are - continuous. -*/ vpColVector vpServo::computeControlLaw() { vpVelocityTwistMatrix cVa; // Twist transformation matrix @@ -936,8 +709,8 @@ vpColVector vpServo::computeControlLaw() if (testInitialization() == false) { vpERROR_TRACE("All the matrices are not correctly initialized"); throw(vpServoException(vpServoException::servoError, "Cannot compute control law. " - "All the matrices are not correctly" - "initialized.")); + "All the matrices are not correctly" + "initialized.")); } } if (testUpdated() == false) { @@ -1039,40 +812,6 @@ vpColVector vpServo::computeControlLaw() return e; } -/*! - Compute the control law specified using setServo(). See vpServo::vpServoType - for more details concerning the control laws that are available. The \ref - tutorial-boost-vs is also useful to illustrate the usage of this function. - - To the general form of the control law given in computeControlLaw(), we add - here an additional term that comes from the task sequencing approach - described in \cite Mansard07e equation (17). This additional term allows to - compute continuous velocities by avoiding abrupt changes in the command. - - The form of the control law considered here is the following: - - \f[ - {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf - \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t) \f] - - where : - - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply - to the robot. - - the sign of the control law depends on the eye in hand or eye to hand - configuration. - - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction - matrix and of the robot Jacobian. - - \f$\bf e = (s-s^*)\f$ is the error to regulate. - - \f$t\f$ is the time given as parameter of this method. - - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified - using setMu(). - - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf - \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$. This value is internally stored - either at the first call of this method, or when \e t parameter is set to 0. - - \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ - {{\bf e}(0)}\f$ is refreshed internally. -*/ vpColVector vpServo::computeControlLaw(double t) { vpVelocityTwistMatrix cVa; // Twist transformation matrix @@ -1082,8 +821,8 @@ vpColVector vpServo::computeControlLaw(double t) if (testInitialization() == false) { vpERROR_TRACE("All the matrices are not correctly initialized"); throw(vpServoException(vpServoException::servoError, "Cannot compute control law " - "All the matrices are not correctly" - "initialized")); + "All the matrices are not correctly" + "initialized")); } } if (testUpdated() == false) { @@ -1193,41 +932,6 @@ vpColVector vpServo::computeControlLaw(double t) return e; } -/*! - Compute the control law specified using setServo(). See vpServo::vpServoType - for more details concerning the control laws that are available. - - To the general form of the control law given in computeControlLaw(), we add - here an additional term that comes from the task sequencing approach - described in \cite Mansard07e equation (17). This additional term allows to - compute continuous velocities by avoiding abrupt changes in the command. - - The form of the control law considered here is the following: - - \f[ - {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + \left({\bf \dot - e}(0) \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\right) \exp(-\mu - t) \f] - - where : - - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply - to the robot. - - the sign of the control law depends on the eye in hand or eye to hand - configuration. - - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction - matrix and of the robot Jacobian. - - \f$\bf e = (s-s^*)\f$ is the error to regulate. - - \f$t\f$ is the time given as parameter of this method. - - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified - using setMu(). - - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf - \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$. This value is internally stored - either at the first call of this method, or when \e t parameter is set to 0. - - \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ - {{\bf e}(0)}\f$ is refreshed internally. \param e_dot_init : Initial value - of \f${\bf \dot e}(0)\f$. -*/ vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init) { vpVelocityTwistMatrix cVa; // Twist transformation matrix @@ -1237,8 +941,8 @@ vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init) if (testInitialization() == false) { vpERROR_TRACE("All the matrices are not correctly initialized"); throw(vpServoException(vpServoException::servoError, "Cannot compute control law " - "All the matrices are not correctly" - "initialized")); + "All the matrices are not correctly" + "initialized")); } } if (testUpdated() == false) { @@ -1349,7 +1053,7 @@ vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init) } void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, - const vpColVector &error_, vpMatrix &P_) const + const vpColVector &error_, vpMatrix &P_) const { // Initialization unsigned int n = J1_.getCols(); @@ -1380,77 +1084,6 @@ void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_ return; } -/*! - Compute and return the secondary task vector according to the classic - projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper - \cite Marchand05b) or the new large projection operator (see equation(24) - in the paper \cite Marey:2010). - - \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the - derivative of the secondary task \f${\bf e}_2\f$. \param - useLargeProjectionOperator : if true will be use the large projection - operator, if false the classic one (default). - - \return The secondary task vector. - - If the classic projection operator is used ( useLargeProjectionOperator = - false (default value)) this function return: - - \f[ - ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t} - \f] - - Note that the secondary task vector need than to be added to the primary - task which can be in the general case written as: \f[ - -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})} - \f] - - Otherwise if the new large projection operator is used ( - useLargeProjectionOperator = true ) this function return: - - \f[ - {\bf P}\frac{\partial {\bf e_2}}{\partial t} - \f] - - where - - \f[ - {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ - \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| - {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f] - - with - - \f[ - {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf - J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf - e }{\bf e }^\top{\bf J_{{\bf e }} } \f] - - \warning computeControlLaw() must be call prior to this function since it - updates the projection operators. - - The following sample code shows how to use this method to compute a - secondary task using the classic projection operator: - \code - vpColVector v; - // Velocity applied to the robot vpColVector de2dt; vpServo task; - ... - v = task.computeControlLaw(); // Compute the primary task - v += task.secondaryTask(de2dt) // Compute and add the secondary task using the classical projection operator - \endcode - - The following sample code shows how to use this method to compute a - secondary task using the large projection operator: - \code - vpColVector v; - // Velocity applied to the robot vpColVector de2dt; vpServo task; - ... - v = task.computeControlLaw(); // Compute the primary task - v += task.secondaryTask(de2dt, true) // Compute and add the secondary task using the large projection operator - \endcode - - \sa computeControlLaw() -*/ vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator) { vpColVector sec; @@ -1483,83 +1116,8 @@ vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLarg return sec; } -/*! - Compute and return the secondary task vector according to the classic - projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper - \cite Marchand05b) or the new large projection operator (see equation(24) - in the paper \cite Marey:2010). - - \param e2 : Value of the secondary task \f${\bf e}_2\f$. - \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the - derivative of the secondary task \f${\bf e}_2\f$. \param - useLargeProjectionOperator: if true will be use the large projection - operator, if false the classic one (default). - - \return The secondary task vector. - - If the classic projection operator is used ( useLargeProjectionOperator = - false (default value)) this function return: - - \f[ - -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf - e_2}}{\partial t} \f] - - Note that the secondary task vector need than to be added to the primary - task which can be in the general case written as: \f[ - -\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})} - \f] - - - Otherwise if the new large projection operator is used ( - useLargeProjectionOperator = true ) this function return: - - \f[ - -\lambda {\bf P} {\bf e_2} + {\bf P}\frac{\partial {\bf e_2}}{\partial t} - \f] - - where - - \f[ - {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ - \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| - {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f] - - with - - \f[ - {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf - J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf - e }{\bf e }^\top{\bf J_{{\bf e }} } \f] - - \warning computeControlLaw() must be call prior to this function since it - updates the projection operators. - - The following sample code shows how to use this method to compute a - secondary task using the classical projection operator: - \code - vpColVector v; - // Velocity applied to the robot vpColVector e2; vpColVector de2dt; vpServo - task; - ... - v = task.computeControlLaw(); // Compute the primary task - v += task.secondaryTask(e2, de2dt) // Compute and add the secondary task using the classical projection operator - \endcode - - The following sample code shows how to use this method to compute a - secondary task using the large projection operator: - \code - vpColVector v; - // Velocity applied to the robot vpColVector e2; vpColVector de2dt; vpServo - task; - ... - v = task.computeControlLaw(); // Compute the primary task - v += task.secondaryTask(e2, de2dt, true) // Compute and add the secondary task using the large projection operator - \endcode - - \sa computeControlLaw() -*/ vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2dt, - const bool &useLargeProjectionOperator) + const bool &useLargeProjectionOperator) { vpColVector sec; @@ -1594,53 +1152,9 @@ vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2 return sec; } -/*! - Compute and return the secondary task vector for joint limit avoidance - \cite Marey:2010b using the new large projection operator (see equation(24) - in the paper \cite Marey:2010). The robot avoids the joint limits very - smoothly even when the main task constrains all the robot degrees of freedom. - - \param q : Actual joint positions vector - - \param dq : Actual joint velocities vector - - \param qmin : Vector containing the low limit value of each joint in the chain. - \param qmax : Vector containing the high limit value of each joint in the chain. - - \param rho : tuning paramenter \f${\left [ 0,\frac{1}{2} \right]}\f$ - used to define the safe configuration for the joint. When the joint - angle value cross the max or min boundaries (\f${ q_{l_{0}}^{max} }\f$ and - \f${q_{l_{0}}^{min}}\f$) the secondary task is actived gradually. - - \param rho1 : tuning paramenter \f${\left ] 0,1 \right ]}\f$ to compute the external - boundaries (\f${q_{l_{1}}^{max}}\f$ and \f${q_{l_{1}}^{min}}\f$) for the joint - limits. Here the secondary task it completely activated with the highest gain. - - \param lambda_tune : value \f${\left [ 0,1 \right ]}\f$ used to tune the - difference in magnitude between the absolute value of the elements of the - primary task and the elements of the secondary task. (See equation (17) - \cite Marey:2010b ) - - \code - vpServo task; - vpColVector qmin; - vpColVector qmax; - vpColVector q; - vpColVector dq; - // Fill vector qmin and qmax with min and max limits of the joints (same joint order than vector q). - // Update vector of joint position q and velocities dq; - ... - // Compute the velocity corresponding to the visual servoing - vpColVector v = task.computeControlLaw(); - // Compute and add the secondary task for the joint limit avoidance - // using the large projection operator - v += task.secondaryTaskJointLimitAvoidance(q, dq, qmin, qmax) - \endcode - - */ vpColVector vpServo::secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, - const vpColVector &qmin, const vpColVector &qmax, - const double &rho, const double &rho1, const double &lambda_tune) + const vpColVector &qmin, const vpColVector &qmax, + const double &rho, const double &rho1, const double &lambda_tune) { unsigned int const n = J1.getCols(); @@ -1656,7 +1170,7 @@ vpColVector vpServo::secondaryTaskJointLimitAvoidance(const vpColVector &q, cons vpERROR_TRACE("Dimension vector q or dq does not correspont to the " "number of jacobian columns"); throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to " - "the number of jacobian columns")); + "the number of jacobian columns")); } double lambda_l = 0.0; @@ -1719,116 +1233,3 @@ vpColVector vpServo::secondaryTaskJointLimitAvoidance(const vpColVector &q, cons } return q2; } - -/*! - Return the projection operator \f${\bf I}-{\bf W}^+{\bf W}\f$. This - operator is updated after a call of computeControlLaw(). - - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing - vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator - \endcode - \sa getWpW() - */ -vpMatrix vpServo::getI_WpW() const { return I_WpW; } - -/*! - Return the large projection operator. This operator is updated - after a call of computeControlLaw(). - - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing - vpMatrix P = task.getP(); // Get the large projection operator - \endcode - \sa getP() - */ -vpMatrix vpServo::getLargeP() const { return P; } - -/*! - Return the task jacobian \f$J\f$. The task jacobian is updated after a call - of computeControlLaw(). - - In the general case, the task jacobian is given by \f${\bf J} = {\widehat - {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$. - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing vpMatrix - J = task.getTaskJacobian(); // Get the task jacobian used to compute v - \endcode - \sa getTaskJacobianPseudoInverse(), getInteractionMatrix() - */ -vpMatrix vpServo::getTaskJacobian() const { return J1; } - -/*! - Return the pseudo inverse of the task jacobian \f$J\f$. - - In the general case, the task jacobian is given by \f${\bf J} = {\widehat - {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$. - - The task jacobian and its pseudo inverse are updated after a call of computeControlLaw(). - - \return Pseudo inverse \f${J}^{+}\f$ of the task jacobian. - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing - vpMatrix Jp = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v - \endcode - - \sa getTaskJacobian() - */ -vpMatrix vpServo::getTaskJacobianPseudoInverse() const { return J1p; } -/*! - Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw(). - - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing - unsigned int rank = task.getTaskRank(); // Get the rank of the task jacobian - \endcode - */ -unsigned int vpServo::getTaskRank() const { return rankJ1; } - -/*! - Return the projection operator \f${\bf W}^+{\bf W}\f$. This operator is - updated after a call of computeControlLaw(). - - When the dimension of the task is equal to the number of degrees of freedom - available \f${\bf W^+W = I}\f$. - - \code - vpServo task; - ... - vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing - vpMatrix WpW = task.getWpW(); // Get the projection operator - \endcode - \sa getI_WpW() - */ -vpMatrix vpServo::getWpW() const { return WpW; } - -/*! - * Return pseudo-inverse threshold used to test the singular values. If - * a singular value is lower than this threshold we consider that the - * matrix is not full rank. - * - * \sa setPseudoInverseThreshold() - */ -double vpServo::getPseudoInverseThreshold() const { return m_pseudo_inverse_threshold; } - -/*! - * Set the pseudo-inverse threshold used to test the singular values. If - * a singular value is lower than this threshold we consider that the - * matrix is not full rank. - * \param pseudo_inverse_threshold : Value to use. Default value is set to 1e-6. - * \sa getPseudoInverseThreshold() - */ -void vpServo::setPseudoInverseThreshold(double pseudo_inverse_threshold) -{ - m_pseudo_inverse_threshold = pseudo_inverse_threshold; -} diff --git a/modules/vs/src/vpServoData.cpp b/modules/vs/src/vpServoData.cpp index 8d67f81a4f..e29008f74e 100644 --- a/modules/vs/src/vpServoData.cpp +++ b/modules/vs/src/vpServoData.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Save data during the task execution. - * -*****************************************************************************/ + */ /*! \file vpServoData.cpp @@ -94,9 +92,19 @@ void vpServoData::save(const vpServo &task) void vpServoData::close() { - velocityFile.close(); - errorFile.close(); - errorNormFile.close(); - sFile.close(); - sStarFile.close(); + if (velocityFile.is_open()) { + velocityFile.close(); + } + if (errorFile.is_open()) { + errorFile.close(); + } + if (errorNormFile.is_open()) { + errorNormFile.close(); + } + if (sFile.is_open()) { + sFile.close(); + } + if (sStarFile.is_open()) { + sStarFile.close(); + } } diff --git a/modules/vs/src/vpServoDisplay.cpp b/modules/vs/src/vpServoDisplay.cpp index 2710c5519c..137f8f6244 100644 --- a/modules/vs/src/vpServoDisplay.cpp +++ b/modules/vs/src/vpServoDisplay.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Interface with the image for feature display. - * -*****************************************************************************/ + */ /*! \file vpServoDisplay.cpp @@ -47,33 +45,10 @@ // Color / image / display #include #include - #include - #include - #include -/*! - - Display the current and the desired features in the image I. - - \warning To effectively display the dot graphics a call to - vpDisplay::flush() is needed. - - \param s : Visual servoing control law. - \param cam : Camera parameters. - \param I : Image on which features have to be displayed. - - \param currentColor : Color for the current features. If vpColor::none, - current features display is turned off. - - \param desiredColor : Color for the desired features. If vpColor::none, - desired features display is turned off. - - \param thickness : Thickness of the feature representation. - -*/ void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, vpColor currentColor, vpColor desiredColor, unsigned int thickness) { @@ -91,29 +66,8 @@ void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, co (*it_s)->display(cam, I, currentColor, thickness); } } - // vpDisplay::flush(I) ; } -/*! - - Display the current and the desired features in the image I. - - \warning To effectively display the dot graphics a call to - vpDisplay::flush() is needed. - - \param s : Visual servoing control law. - \param cam : Camera parameters. - \param I : Color image on which features have to be displayed. - - \param currentColor : Color for the current features. If vpColor::none, - current features display is turned off. - - \param desiredColor : Color for the desired features. If vpColor::none, - desired features display is turned off. - - \param thickness : Thickness of the feature representation. - - */ void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, vpColor currentColor, vpColor desiredColor, unsigned int thickness) { @@ -131,5 +85,4 @@ void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, co (*it_s)->display(cam, I, currentColor, thickness); } } - // vpDisplay::flush(I) ; } diff --git a/tutorial/bridge/opencv/tutorial-bridge-opencv-image.cpp b/tutorial/bridge/opencv/tutorial-bridge-opencv-image.cpp index 77bd225aad..09434f9c52 100644 --- a/tutorial/bridge/opencv/tutorial-bridge-opencv-image.cpp +++ b/tutorial/bridge/opencv/tutorial-bridge-opencv-image.cpp @@ -89,4 +89,4 @@ int main() std::cout << "This tutorial required OpenCV imgproc and imgcodecs modules." << std::endl; return EXIT_SUCCESS; } -#endif \ No newline at end of file +#endif diff --git a/tutorial/grabber/tutorial-grabber-rgbd-D435-structurecore.cpp b/tutorial/grabber/tutorial-grabber-rgbd-D435-structurecore.cpp index 07606908a3..22805544c2 100644 --- a/tutorial/grabber/tutorial-grabber-rgbd-D435-structurecore.cpp +++ b/tutorial/grabber/tutorial-grabber-rgbd-D435-structurecore.cpp @@ -8,7 +8,7 @@ #include /*! - Grab color and depth images from Intel RealSense D435 and Occipital Structure Core sensors. + * Grab color and depth images from Intel RealSense D435 and Occipital Structure Core sensors. */ int main(int argc, char **argv) { @@ -28,9 +28,9 @@ int main(int argc, char **argv) vpImage I_color_sc(height, width), I_color_rs(height, width); vpDisplayX d_rs_depth(I_depth_rs, 10, height + 10, "RealSense Depth"), - d_sc_depth(I_depth_sc, width + 10, height + 10, "Structure Core Depth"); + d_sc_depth(I_depth_sc, width + 10, height + 10, "Structure Core Depth"); vpDisplayX d_color_rs(I_color_rs, 10, 10, "RealSense Color"), - d_color_sc(I_color_sc, width + 10, 10, "Structure Core Color"); + d_color_sc(I_color_sc, width + 10, 10, "Structure Core Color"); // Configuring and opening RealSense grabber. rs2::config cfg; @@ -84,4 +84,4 @@ int main(int argc, char **argv) std::cout << "This tutorial should be built with c++11 support" << std::endl; #endif #endif -} \ No newline at end of file +} diff --git a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp index b0a0061ca7..c7837498dd 100644 --- a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp +++ b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp @@ -199,4 +199,4 @@ void ClassUsingPclViewer::threadedMode(const double &addedNoise, const unsigned #else void dummy_class_using_pcl_visualizer() { } -#endif \ No newline at end of file +#endif diff --git a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h index 091f07c8ba..cbfa0543d9 100644 --- a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h +++ b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h @@ -65,4 +65,4 @@ class ClassUsingPclViewer void threadedMode(const double &addedNoise, const unsigned int &order); }; #endif -#endif \ No newline at end of file +#endif diff --git a/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera.xcodeproj/project.pbxproj b/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera.xcodeproj/project.pbxproj index 44a6ec1824..2b35534f7a 100644 --- a/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera.xcodeproj/project.pbxproj +++ b/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera.xcodeproj/project.pbxproj @@ -16,7 +16,7 @@ 3040DA5B221F991300C72347 /* VispDetector.mm in Sources */ = {isa = PBXBuildFile; fileRef = 3040DA58221F991300C72347 /* VispDetector.mm */; }; 3040DA61221F99DC00C72347 /* ImageConversion.mm in Sources */ = {isa = PBXBuildFile; fileRef = 3040DA5E221F99DC00C72347 /* ImageConversion.mm */; }; 3040DA62221F99DC00C72347 /* ImageDisplay.mm in Sources */ = {isa = PBXBuildFile; fileRef = 3040DA60221F99DC00C72347 /* ImageDisplay.mm */; }; - 30E9DA38222E16B50044ACD3 /* ImageDisplay+withContext.mm in Sources */ = {isa = PBXBuildFile; fileRef = 30E9DA37222E16B50044ACD3 /* ImageDisplay+withContext.mm */; }; + 30E9DA38222E16B50044ACD3 /* ImageDisplayWithContext.mm in Sources */ = {isa = PBXBuildFile; fileRef = 30E9DA37222E16B50044ACD3 /* ImageDisplayWithContext.mm */; }; /* End PBXBuildFile section */ /* Begin PBXFileReference section */ @@ -36,8 +36,8 @@ 3040DA60221F99DC00C72347 /* ImageDisplay.mm */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.objcpp; path = ImageDisplay.mm; sourceTree = ""; }; 3040DA64221F9A3000C72347 /* opencv2.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = opencv2.framework; path = "../../../../Library/visp/visp-3.1.0.framework/opencv2.framework"; sourceTree = ""; }; 3040DA65221F9A3000C72347 /* visp3.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = visp3.framework; path = "../../../../Library/visp/visp-3.1.0.framework/visp3.framework"; sourceTree = ""; }; - 30E9DA36222E16B50044ACD3 /* ImageDisplay+withContext.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = "ImageDisplay+withContext.h"; sourceTree = ""; }; - 30E9DA37222E16B50044ACD3 /* ImageDisplay+withContext.mm */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.objcpp; path = "ImageDisplay+withContext.mm"; sourceTree = ""; }; + 30E9DA36222E16B50044ACD3 /* ImageDisplayWithContext.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = "ImageDisplayWithContext.h"; sourceTree = ""; }; + 30E9DA37222E16B50044ACD3 /* ImageDisplayWithContext.mm */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.objcpp; path = "ImageDisplayWithContext.mm"; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ @@ -100,8 +100,8 @@ 3040DA5E221F99DC00C72347 /* ImageConversion.mm */, 3040DA5F221F99DC00C72347 /* ImageDisplay.h */, 3040DA60221F99DC00C72347 /* ImageDisplay.mm */, - 30E9DA36222E16B50044ACD3 /* ImageDisplay+withContext.h */, - 30E9DA37222E16B50044ACD3 /* ImageDisplay+withContext.mm */, + 30E9DA36222E16B50044ACD3 /* ImageDisplayWithContext.h */, + 30E9DA37222E16B50044ACD3 /* ImageDisplayWithContext.mm */, ); path = VispHelper; sourceTree = ""; @@ -193,7 +193,7 @@ 3040DA61221F99DC00C72347 /* ImageConversion.mm in Sources */, 3040DA46221F988200C72347 /* ViewController.swift in Sources */, 3040DA44221F988200C72347 /* AppDelegate.swift in Sources */, - 30E9DA38222E16B50044ACD3 /* ImageDisplay+withContext.mm in Sources */, + 30E9DA38222E16B50044ACD3 /* ImageDisplayWithContext.mm in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera/VispDetector.mm b/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera/VispDetector.mm index 1274fe414f..eac2c49962 100644 --- a/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera/VispDetector.mm +++ b/tutorial/ios/AprilTagLiveCamera/AprilTagLiveCamera/VispDetector.mm @@ -6,14 +6,14 @@ #import "VispDetector.h" #import "ImageConversion.h" -#import "ImageDisplay+withContext.h" +#import "ImageDisplayWithContext.h" vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11, vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); @implementation VispDetector - (UIImage *)detectAprilTag:(UIImage *)image px:(float)px py:(float)py { - + // make vpImage for the detection. vpImage I = [ImageConversion vpImageGrayFromUIImage:image]; @@ -25,50 +25,50 @@ - (UIImage *)detectAprilTag:(UIImage *)image px:(float)px py:(float)py { px = 1515.0; py = 1515.0; } - + // AprilTag detections setting float quadDecimate = 3.0; int nThreads = 1; double tagSize = 0.043; // meter detector.setAprilTagQuadDecimate(quadDecimate); detector.setAprilTagNbThreads(nThreads); - + // Detection. vpCameraParameters cam; cam.initPersProjWithoutDistortion(px,py,u0,v0); std::vector cMo_vec; detector.detect(I, tagSize, cam, cMo_vec); - + // starts drawing UIGraphicsBeginImageContext(image.size); CGContextRef context = UIGraphicsGetCurrentContext(); - + // draw original image in the current context. [image drawAtPoint:CGPointMake(0,0)]; - + // draw frames by each tag. int tagNums = (int) detector.getNbObjects(); for(int i=0; i < tagNums; i++){ - + // parameters std::vector polygon = detector.getPolygon(i); vpImagePoint cog = detector.getCog(i); vpTranslationVector trans = cMo_vec[i].getTranslationVector(); UIColor *mainColor = [UIColor blueColor]; int tagLineWidth = 10; - + // tag Id from message: "36h11 id: 1" -> 1 NSString * message = [NSString stringWithCString:detector.getMessage(i).c_str() encoding:[NSString defaultCStringEncoding]]; NSArray *phases = [message componentsSeparatedByString:@" "]; int detectedTagId = [phases[2] intValue]; - + // draw tag id NSString *tagIdStr = [NSString stringWithFormat:@"%d", detectedTagId]; [ImageDisplay displayText:tagIdStr :polygon[0].get_u() :polygon[0].get_v() - 50 :600 :100 :mainColor :[UIColor clearColor]]; - + // draw tag frame [ImageDisplay displayLineWithContext:context :polygon :mainColor :tagLineWidth]; - + // draw xyz cordinate. [ImageDisplay displayFrameWithContext:context :cMo_vec[i] :cam :tagSize :6]; @@ -76,10 +76,10 @@ - (UIImage *)detectAprilTag:(UIImage *)image px:(float)px py:(float)py { NSString *meter = [NSString stringWithFormat:@"(%.2f,%.2f,%.2f)",trans[0],trans[1],trans[2]]; [ImageDisplay displayText:meter :cog.get_u() :cog.get_v() +50 :600 :100 :[UIColor whiteColor] :[UIColor blueColor]]; } - + image = UIGraphicsGetImageFromCurrentImageContext(); UIGraphicsEndImageContext(); - + return image; } diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay+withContext.h b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h similarity index 100% rename from tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay+withContext.h rename to tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay+withContext.mm b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm similarity index 99% rename from tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay+withContext.mm rename to tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm index 60ad17e5a4..9c1dc3b57a 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay+withContext.mm +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm @@ -30,7 +30,7 @@ * *****************************************************************************/ -#import "ImageDisplay+withContext.h" +#import "ImageDisplayWithContext.h" @implementation ImageDisplay (withContext) @@ -125,4 +125,3 @@ + (void)displayText:(NSString*)text :(double)x :(double)y :(int)width :(int)heig //! [display text] @end - diff --git a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp index 0818874ced..94ac5f992d 100644 --- a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp +++ b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp @@ -278,7 +278,7 @@ int main(int argc, const char **argv) mg.compute(); // Compute gravity center moment mc.compute(); // Compute centered moments AFTER gravity center man.setDesiredArea( - area); // Since desired area was init at 0, because unknow at contruction, need to be updated here + area); // Since desired area was init at 0, because unknow at construction, need to be updated here man.compute(); // Compute area normalized moment AFTER centered moment mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment diff --git a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp index 3d1bd1479d..017e2b1cc3 100644 --- a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp +++ b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp @@ -17,7 +17,7 @@ namespace { -struct rs_intrinsics { +struct vpRealsenseIntrinsics_t { float ppx; /**< Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ float ppy; /**< Vertical coordinate of the principal point of the image, as @@ -29,7 +29,7 @@ struct rs_intrinsics { float coeffs[5]; /**< Distortion coefficients */ }; -void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth) +void rs_deproject_pixel_to_point(float point[3], const vpRealsenseIntrinsics_t &intrin, const float pixel[2], float depth) { float x = (pixel[0] - intrin.ppx) / intrin.fx; float y = (pixel[1] - intrin.ppy) / intrin.fy; @@ -96,7 +96,7 @@ bool read_data(unsigned int cpt, const std::string &input_directory, vpImage