From 3c25fd1ba59504c563cb00134f8785cf4d10811a Mon Sep 17 00:00:00 2001 From: Christos Malliaridis Date: Tue, 14 Jul 2020 18:44:12 +0200 Subject: [PATCH 1/6] Update and expand erosion / dilation tutorial - Add python explanation for erosion and dilation - Add java explanation for erosion and dilation - Restructure and reword specific sections --- .../erosion_dilatation.markdown | 216 +++++++++++++++--- .../tutorial_code/ImgProc/Morphology_1.cpp | 2 + .../erosion_dilatation/MorphologyDemo1.java | 14 ++ .../erosion_dilatation/morphology_1.py | 95 ++++---- 4 files changed, 250 insertions(+), 77 deletions(-) diff --git a/doc/tutorials/imgproc/erosion_dilatation/erosion_dilatation.markdown b/doc/tutorials/imgproc/erosion_dilatation/erosion_dilatation.markdown index ddb7d9e8f5..adcedb2539 100644 --- a/doc/tutorials/imgproc/erosion_dilatation/erosion_dilatation.markdown +++ b/doc/tutorials/imgproc/erosion_dilatation/erosion_dilatation.markdown @@ -84,57 +84,198 @@ This tutorial's code is shown below. You can also download it Explanation ----------- --# Most of the material shown here is trivial (if you have any doubt, please refer to the tutorials in - previous sections). Let's check the general structure of the C++ program: +@add_toggle_cpp +Most of the material shown here is trivial (if you have any doubt, please refer to the tutorials in +previous sections). Let's check the general structure of the C++ program: - - Load an image (can be BGR or grayscale) - - Create two windows (one for dilation output, the other for erosion) - - Create a set of two Trackbars for each operation: - - The first trackbar "Element" returns either **erosion_elem** or **dilation_elem** - - The second trackbar "Kernel size" return **erosion_size** or **dilation_size** for the - corresponding operation. - - Every time we move any slider, the user's function **Erosion** or **Dilation** will be - called and it will update the output image based on the current trackbar values. +@snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp main - Let's analyze these two functions: +-# Load an image (can be BGR or grayscale) +-# Create two windows (one for dilation output, the other for erosion) +-# Create a set of two Trackbars for each operation: + - The first trackbar "Element" returns either **erosion_elem** or **dilation_elem** + - The second trackbar "Kernel size" return **erosion_size** or **dilation_size** for the + corresponding operation. +-# Call once erosion and dilation to show the initial image. --# **erosion:** - @snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp erosion - - The function that performs the *erosion* operation is @ref cv::erode . As we can see, it - receives three arguments: - - *src*: The source image - - *erosion_dst*: The output image - - *element*: This is the kernel we will use to perform the operation. If we do not - specify, the default is a simple `3x3` matrix. Otherwise, we can specify its - shape. For this, we need to use the function cv::getStructuringElement : - @snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp kernel +Every time we move any slider, the user's function **Erosion** or **Dilation** will be +called and it will update the output image based on the current trackbar values. - We can choose any of three shapes for our kernel: +Let's analyze these two functions: - - Rectangular box: MORPH_RECT - - Cross: MORPH_CROSS - - Ellipse: MORPH_ELLIPSE +#### The erosion function - Then, we just have to specify the size of our kernel and the *anchor point*. If not - specified, it is assumed to be in the center. +@snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp erosion - - That is all. We are ready to perform the erosion of our image. -@note Additionally, there is another parameter that allows you to perform multiple erosions -(iterations) at once. However, We haven't used it in this simple tutorial. You can check out the -reference for more details. +The function that performs the *erosion* operation is @ref cv::erode . As we can see, it +receives three arguments: +- *src*: The source image +- *erosion_dst*: The output image +- *element*: This is the kernel we will use to perform the operation. If we do not + specify, the default is a simple `3x3` matrix. Otherwise, we can specify its + shape. For this, we need to use the function cv::getStructuringElement : + @snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp kernel --# **dilation:** + We can choose any of three shapes for our kernel: - The code is below. As you can see, it is completely similar to the snippet of code for **erosion**. - Here we also have the option of defining our kernel, its anchor point and the size of the operator - to be used. - @snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp dilation + - Rectangular box: MORPH_RECT + - Cross: MORPH_CROSS + - Ellipse: MORPH_ELLIPSE + + Then, we just have to specify the size of our kernel and the *anchor point*. If not + specified, it is assumed to be in the center. + +That is all. We are ready to perform the erosion of our image. + +#### The dilation function + +The code is below. As you can see, it is completely similar to the snippet of code for **erosion**. +Here we also have the option of defining our kernel, its anchor point and the size of the operator +to be used. +@snippet cpp/tutorial_code/ImgProc/Morphology_1.cpp dilation +@end_toggle + +@add_toggle_java +Most of the material shown here is trivial (if you have any doubt, please refer to the tutorials in +previous sections). Let's check however the general structure of the java class. There are 4 main +parts in the java class: + +- the class constructor which setups the window that will be filled with window components +- the `addComponentsToPane` method, which fills out the window +- the `update` method, which determines what happens when the user changes any value +- the `main` method, which is the entry point of the program + +In this tutorial we will focus on the `addComponentsToPane` and `update` methods. However, for completion the +steps followed in the constructor are: + +-# Load an image (can be BGR or grayscale) +-# Create a window +-# Add various control components with `addComponentsToPane` +-# show the window + +The components were added by the following method: + +@snippet java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java components + +In short we + +-# create a panel for the sliders +-# create a combo box for the element types +-# create a slider for the kernel size +-# create a combo box for the morphology function to use (erosion or dilation) + +The action and state changed listeners added call at the end the `update` method which updates +the image based on the current slider values. So every time we move any slider, the `update` method is triggered. + +#### Updating the image + +To update the image we used the following implementation: + +@snippet java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java update + +In other words we + +-# get the structuring element the user chose +-# execute the **erosion** or **dilation** function based on `doErosion` +-# reload the image with the morphology applied +-# repaint the frame + +Let's analyze the `erode` and `dilate` methods: + +#### The erosion method + +@snippet java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java erosion + +The function that performs the *erosion* operation is @ref cv::erode . As we can see, it +receives three arguments: +- *src*: The source image +- *erosion_dst*: The output image +- *element*: This is the kernel we will use to perform the operation. For specifying the shape, we need to use + the function cv::getStructuringElement : + @snippet java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java kernel + + We can choose any of three shapes for our kernel: + + - Rectangular box: CV_SHAPE_RECT + - Cross: CV_SHAPE_CROSS + - Ellipse: CV_SHAPE_ELLIPSE + + Together with the shape we specify the size of our kernel and the *anchor point*. If the anchor point is not + specified, it is assumed to be in the center. + +That is all. We are ready to perform the erosion of our image. + +#### The dilation function + +The code is below. As you can see, it is completely similar to the snippet of code for **erosion**. +Here we also have the option of defining our kernel, its anchor point and the size of the operator +to be used. +@snippet java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java dilation +@end_toggle + +@add_toggle_python +Most of the material shown here is trivial (if you have any doubt, please refer to the tutorials in +previous sections). Let's check the general structure of the python script: + +@snippet python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py main + +-# Load an image (can be BGR or grayscale) +-# Create two windows (one for erosion output, the other for dilation) with a set of trackbars each + - The first trackbar "Element" returns the value for the morphological type that will be mapped + (1 = rectangle, 2 = cross, 3 = ellipse) + - The second trackbar "Kernel size" returns the size of the element for the + corresponding operation +-# Call once erosion and dilation to show the initial image + +Every time we move any slider, the user's function **erosion** or **dilation** will be +called and it will update the output image based on the current trackbar values. + +Let's analyze these two functions: + +#### The erosion function + +@snippet python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py erosion + +The function that performs the *erosion* operation is @ref cv::erode . As we can see, it +receives two arguments and returns the processed image: +- *src*: The source image +- *element*: The kernel we will use to perform the operation. We can specify its + shape by using the function cv::getStructuringElement : + @snippet python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py kernel + + We can choose any of three shapes for our kernel: + + - Rectangular box: MORPH_RECT + - Cross: MORPH_CROSS + - Ellipse: MORPH_ELLIPSE + +Then, we just have to specify the size of our kernel and the *anchor point*. If the anchor point not +specified, it is assumed to be in the center. + +That is all. We are ready to perform the erosion of our image. + +#### The dilation function + +The code is below. As you can see, it is completely similar to the snippet of code for **erosion**. +Here we also have the option of defining our kernel, its anchor point and the size of the operator +to be used. + +@snippet python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py dilation +@end_toggle + +@note Additionally, there are further parameters that allow you to perform multiple erosions/dilations +(iterations) at once and also set the border type and value. However, We haven't used those +in this simple tutorial. You can check out the reference for more details. Results ------- -Compile the code above and execute it with an image as argument. For instance, using this image: +Compile the code above and execute it (or run the script if using python) with an image as argument. +If you do not provide an image as argument the default sample image +([LinuxLogo.jpg](https://github.com/opencv/opencv/tree/master/samples/data/LinuxLogo.jpg)) will be used. + +For instance, using this image: ![](images/Morphology_1_Tutorial_Original_Image.jpg) @@ -143,3 +284,4 @@ naturally. Try them out! You can even try to add a third Trackbar to control the iterations. ![](images/Morphology_1_Result.jpg) +(depending on the programming language the output might vary a little or be only 1 window) diff --git a/samples/cpp/tutorial_code/ImgProc/Morphology_1.cpp b/samples/cpp/tutorial_code/ImgProc/Morphology_1.cpp index 48b0c2e6e3..33e006269d 100644 --- a/samples/cpp/tutorial_code/ImgProc/Morphology_1.cpp +++ b/samples/cpp/tutorial_code/ImgProc/Morphology_1.cpp @@ -25,6 +25,7 @@ int const max_kernel_size = 21; void Erosion( int, void* ); void Dilation( int, void* ); +//![main] /** * @function main */ @@ -70,6 +71,7 @@ int main( int argc, char** argv ) waitKey(0); return 0; } +//![main] //![erosion] /** diff --git a/samples/java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java b/samples/java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java index 7a5f60f065..e71400f737 100644 --- a/samples/java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java +++ b/samples/java/tutorial_code/ImgProc/erosion_dilatation/MorphologyDemo1.java @@ -34,6 +34,7 @@ public class MorphologyDemo1 { private JFrame frame; private JLabel imgLabel; + //! [constructor] public MorphologyDemo1(String[] args) { String imagePath = args.length > 0 ? args[0] : "../data/LinuxLogo.jpg"; matImgSrc = Imgcodecs.imread(imagePath); @@ -54,7 +55,9 @@ public class MorphologyDemo1 { frame.pack(); frame.setVisible(true); } + //! [constructor] + //! [components] private void addComponentsToPane(Container pane, Image img) { if (!(pane.getLayout() instanceof BorderLayout)) { pane.add(new JLabel("Container doesn't use BorderLayout!")); @@ -114,21 +117,31 @@ public class MorphologyDemo1 { imgLabel = new JLabel(new ImageIcon(img)); pane.add(imgLabel, BorderLayout.CENTER); } + //! [components] + //! [update] private void update() { + //! [kernel] Mat element = Imgproc.getStructuringElement(elementType, new Size(2 * kernelSize + 1, 2 * kernelSize + 1), new Point(kernelSize, kernelSize)); + //! [kernel] if (doErosion) { + //! [erosion] Imgproc.erode(matImgSrc, matImgDst, element); + //! [erosion] } else { + //! [dilation] Imgproc.dilate(matImgSrc, matImgDst, element); + //! [dilation] } Image img = HighGui.toBufferedImage(matImgDst); imgLabel.setIcon(new ImageIcon(img)); frame.repaint(); } + //! [update] + //! [main] public static void main(String[] args) { // Load the native OpenCV library System.loadLibrary(Core.NATIVE_LIBRARY_NAME); @@ -142,4 +155,5 @@ public class MorphologyDemo1 { } }); } + //! [main] } diff --git a/samples/python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py b/samples/python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py index 502457b471..3645eab3d1 100644 --- a/samples/python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py +++ b/samples/python/tutorial_code/imgProc/erosion_dilatation/morphology_1.py @@ -3,61 +3,76 @@ import cv2 as cv import numpy as np import argparse +src = None erosion_size = 0 max_elem = 2 max_kernel_size = 21 -title_trackbar_element_type = 'Element:\n 0: Rect \n 1: Cross \n 2: Ellipse' +title_trackbar_element_shape = 'Element:\n 0: Rect \n 1: Cross \n 2: Ellipse' title_trackbar_kernel_size = 'Kernel size:\n 2n +1' title_erosion_window = 'Erosion Demo' -title_dilatation_window = 'Dilation Demo' +title_dilation_window = 'Dilation Demo' + +## [main] +def main(image): + global src + src = cv.imread(cv.samples.findFile(image)) + if src is None: + print('Could not open or find the image: ', image) + exit(0) + + cv.namedWindow(title_erosion_window) + cv.createTrackbar(title_trackbar_element_shape, title_erosion_window, 0, max_elem, erosion) + cv.createTrackbar(title_trackbar_kernel_size, title_erosion_window, 0, max_kernel_size, erosion) + + cv.namedWindow(title_dilation_window) + cv.createTrackbar(title_trackbar_element_shape, title_dilation_window, 0, max_elem, dilatation) + cv.createTrackbar(title_trackbar_kernel_size, title_dilation_window, 0, max_kernel_size, dilatation) + + erosion(0) + dilatation(0) + cv.waitKey() +## [main] + +# optional mapping of values with morphological shapes +def morph_shape(val): + if val == 0: + return cv.MORPH_RECT + elif val == 1: + return cv.MORPH_CROSS + elif val == 2: + return cv.MORPH_ELLIPSE + + +## [erosion] def erosion(val): erosion_size = cv.getTrackbarPos(title_trackbar_kernel_size, title_erosion_window) - erosion_type = 0 - val_type = cv.getTrackbarPos(title_trackbar_element_type, title_erosion_window) - if val_type == 0: - erosion_type = cv.MORPH_RECT - elif val_type == 1: - erosion_type = cv.MORPH_CROSS - elif val_type == 2: - erosion_type = cv.MORPH_ELLIPSE + erosion_shape = morph_shape(cv.getTrackbarPos(title_trackbar_element_shape, title_erosion_window)) - element = cv.getStructuringElement(erosion_type, (2*erosion_size + 1, 2*erosion_size+1), (erosion_size, erosion_size)) + ## [kernel] + element = cv.getStructuringElement(erosion_shape, (2 * erosion_size + 1, 2 * erosion_size + 1), + (erosion_size, erosion_size)) + ## [kernel] erosion_dst = cv.erode(src, element) cv.imshow(title_erosion_window, erosion_dst) +## [erosion] + +## [dilation] def dilatation(val): - dilatation_size = cv.getTrackbarPos(title_trackbar_kernel_size, title_dilatation_window) - dilatation_type = 0 - val_type = cv.getTrackbarPos(title_trackbar_element_type, title_dilatation_window) - if val_type == 0: - dilatation_type = cv.MORPH_RECT - elif val_type == 1: - dilatation_type = cv.MORPH_CROSS - elif val_type == 2: - dilatation_type = cv.MORPH_ELLIPSE + dilatation_size = cv.getTrackbarPos(title_trackbar_kernel_size, title_dilation_window) + dilation_shape = morph_shape(cv.getTrackbarPos(title_trackbar_element_shape, title_dilation_window)) - element = cv.getStructuringElement(dilatation_type, (2*dilatation_size + 1, 2*dilatation_size+1), (dilatation_size, dilatation_size)) + element = cv.getStructuringElement(dilation_shape, (2 * dilatation_size + 1, 2 * dilatation_size + 1), + (dilatation_size, dilatation_size)) dilatation_dst = cv.dilate(src, element) - cv.imshow(title_dilatation_window, dilatation_dst) + cv.imshow(title_dilation_window, dilatation_dst) +## [dilation] -parser = argparse.ArgumentParser(description='Code for Eroding and Dilating tutorial.') -parser.add_argument('--input', help='Path to input image.', default='LinuxLogo.jpg') -args = parser.parse_args() -src = cv.imread(cv.samples.findFile(args.input)) -if src is None: - print('Could not open or find the image: ', args.input) - exit(0) +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Code for Eroding and Dilating tutorial.') + parser.add_argument('--input', help='Path to input image.', default='LinuxLogo.jpg') + args = parser.parse_args() -cv.namedWindow(title_erosion_window) -cv.createTrackbar(title_trackbar_element_type, title_erosion_window , 0, max_elem, erosion) -cv.createTrackbar(title_trackbar_kernel_size, title_erosion_window , 0, max_kernel_size, erosion) - -cv.namedWindow(title_dilatation_window) -cv.createTrackbar(title_trackbar_element_type, title_dilatation_window , 0, max_elem, dilatation) -cv.createTrackbar(title_trackbar_kernel_size, title_dilatation_window , 0, max_kernel_size, dilatation) - -erosion(0) -dilatation(0) -cv.waitKey() + main(args.input) From cc7f17f011083060979b980360e2744607b5972e Mon Sep 17 00:00:00 2001 From: Suleyman TURKMEN Date: Wed, 28 Oct 2020 04:51:12 +0300 Subject: [PATCH 2/6] update documentation --- modules/core/include/opencv2/core.hpp | 9 ++++++ modules/highgui/include/opencv2/highgui.hpp | 29 +++++++++++++------ .../imgcodecs/include/opencv2/imgcodecs.hpp | 6 ++++ 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/modules/core/include/opencv2/core.hpp b/modules/core/include/opencv2/core.hpp index 6eb519e8a2..fc2432dcdf 100644 --- a/modules/core/include/opencv2/core.hpp +++ b/modules/core/include/opencv2/core.hpp @@ -202,6 +202,9 @@ enum CovarFlags { COVAR_COLS = 16 }; +//! @addtogroup core_cluster +//! @{ + //! k-Means flags enum KmeansFlags { /** Select random initial centers in each attempt.*/ @@ -215,6 +218,8 @@ enum KmeansFlags { KMEANS_USE_INITIAL_LABELS = 1 }; +//! @} core_cluster + //! type of line enum LineTypes { FILLED = -1, @@ -236,12 +241,16 @@ enum HersheyFonts { FONT_ITALIC = 16 //!< flag for italic font }; +//! @addtogroup core_array +//! @{ + enum ReduceTypes { REDUCE_SUM = 0, //!< the output is the sum of all rows/columns of the matrix. REDUCE_AVG = 1, //!< the output is the mean vector of all rows/columns of the matrix. REDUCE_MAX = 2, //!< the output is the maximum (column/row-wise) of all rows/columns of the matrix. REDUCE_MIN = 3 //!< the output is the minimum (column/row-wise) of all rows/columns of the matrix. }; +//! @} core_array /** @brief Swaps two matrices */ diff --git a/modules/highgui/include/opencv2/highgui.hpp b/modules/highgui/include/opencv2/highgui.hpp index 628b7fa9ce..6a8c598720 100644 --- a/modules/highgui/include/opencv2/highgui.hpp +++ b/modules/highgui/include/opencv2/highgui.hpp @@ -66,6 +66,7 @@ It provides easy interface to: - Add trackbars to the windows, handle simple mouse events as well as keyboard commands. @{ + @defgroup highgui_window_flags Flags related creating and manipulating HighGUI windows and mouse events @defgroup highgui_opengl OpenGL support @defgroup highgui_qt Qt New Functions @@ -93,7 +94,7 @@ It provides easy interface to: namedWindow("main1",WINDOW_NORMAL); - namedWindow("main2",WINDOW_AUTOSIZE | CV_GUI_NORMAL); + namedWindow("main2",WINDOW_AUTOSIZE | WINDOW_GUI_NORMAL); createTrackbar( "track1", "main1", &value, 255, NULL); String nameb1 = "button1"; @@ -178,6 +179,9 @@ namespace cv //! @addtogroup highgui //! @{ +//! @addtogroup highgui_window_flags +//! @{ + //! Flags for cv::namedWindow enum WindowFlags { WINDOW_NORMAL = 0x00000000, //!< the user can resize the window (no constraint) / also use to switch a fullscreen window to a normal size. @@ -227,6 +231,11 @@ enum MouseEventFlags { EVENT_FLAG_ALTKEY = 32 //!< indicates that ALT Key is pressed. }; +//! @} highgui_window_flags + +//! @addtogroup highgui_qt +//! @{ + //! Qt font weight enum QtFontWeights { QT_FONT_LIGHT = 25, //!< Weight of 25 @@ -251,6 +260,8 @@ enum QtButtonTypes { QT_NEW_BUTTONBAR = 1024 //!< Button should create a new buttonbar }; +//! @} highgui_qt + /** @brief Callback function for mouse events. see cv::setMouseCallback @param event one of the cv::MouseEventTypes constants. @param x The x-coordinate of the mouse event. @@ -389,7 +400,7 @@ videos, it will display the video frame-by-frame) */ CV_EXPORTS_W void imshow(const String& winname, InputArray mat); -/** @brief Resizes window to the specified size +/** @brief Resizes the window to the specified size @note @@ -408,7 +419,7 @@ CV_EXPORTS_W void resizeWindow(const String& winname, int width, int height); */ CV_EXPORTS_W void resizeWindow(const String& winname, const cv::Size& size); -/** @brief Moves window to the specified position +/** @brief Moves the window to the specified position @param winname Name of the window. @param x The new x-coordinate of the window. @@ -476,8 +487,6 @@ For cv::EVENT_MOUSEWHEEL positive and negative values mean forward and backward respectively. For cv::EVENT_MOUSEHWHEEL, where available, positive and negative values mean right and left scrolling, respectively. -With the C API, the macro CV_GET_WHEEL_DELTA(flags) can be used alternatively. - @note Mouse-wheel events are currently supported only on Windows. @@ -486,8 +495,9 @@ Mouse-wheel events are currently supported only on Windows. */ CV_EXPORTS int getMouseWheelDelta(int flags); -/** @brief Selects ROI on the given image. -Function creates a window and allows user to select a ROI using mouse. +/** @brief Allows users to select a ROI on the given image. + +The function creates a window and allows users to select a ROI using the mouse. Controls: use `space` or `enter` to finish selection, use key `c` to cancel selection (function will return the zero cv::Rect). @param windowName name of the window where selection process will be shown. @@ -506,8 +516,9 @@ CV_EXPORTS_W Rect selectROI(const String& windowName, InputArray img, bool showC */ CV_EXPORTS_W Rect selectROI(InputArray img, bool showCrosshair = true, bool fromCenter = false); -/** @brief Selects ROIs on the given image. -Function creates a window and allows user to select a ROIs using mouse. +/** @brief Allows users to select multiple ROIs on the given image. + +The function creates a window and allows users to select multiple ROIs using the mouse. Controls: use `space` or `enter` to finish current selection and start a new one, use `esc` to terminate multiple ROI selection process. diff --git a/modules/imgcodecs/include/opencv2/imgcodecs.hpp b/modules/imgcodecs/include/opencv2/imgcodecs.hpp index e2636e19f7..7f6b24f0ff 100644 --- a/modules/imgcodecs/include/opencv2/imgcodecs.hpp +++ b/modules/imgcodecs/include/opencv2/imgcodecs.hpp @@ -49,6 +49,7 @@ @defgroup imgcodecs Image file reading and writing @{ @defgroup imgcodecs_c C API + @defgroup imgcodecs_flags Flags used for image file reading and writing @defgroup imgcodecs_ios iOS glue @} */ @@ -60,6 +61,9 @@ namespace cv //! @addtogroup imgcodecs //! @{ +//! @addtogroup imgcodecs_flags +//! @{ + //! Imread flags enum ImreadModes { IMREAD_UNCHANGED = -1, //!< If set, return the loaded image as is (with alpha channel, otherwise it gets cropped). Ignore EXIF orientation. @@ -130,6 +134,8 @@ enum ImwritePAMFlags { IMWRITE_PAM_FORMAT_RGB_ALPHA = 5, }; +//! @} imgcodecs_flags + /** @brief Loads an image from a file. @anchor imread From fef23768fe16525d30716f77e440d082ad5683d4 Mon Sep 17 00:00:00 2001 From: Ian Maquignaz <9im14@queensu.ca> Date: Tue, 17 Nov 2020 23:13:57 -0500 Subject: [PATCH 3/6] Fixed issue with Epipolar Geometry Tutorial --- .../py_epipolar_geometry/py_epipolar_geometry.markdown | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/doc/py_tutorials/py_calib3d/py_epipolar_geometry/py_epipolar_geometry.markdown b/doc/py_tutorials/py_calib3d/py_epipolar_geometry/py_epipolar_geometry.markdown index 3ed072c04d..6b8d90882a 100644 --- a/doc/py_tutorials/py_calib3d/py_epipolar_geometry/py_epipolar_geometry.markdown +++ b/doc/py_tutorials/py_calib3d/py_epipolar_geometry/py_epipolar_geometry.markdown @@ -79,7 +79,7 @@ from matplotlib import pyplot as plt img1 = cv.imread('myleft.jpg',0) #queryimage # left image img2 = cv.imread('myright.jpg',0) #trainimage # right image -sift = cv.SIFT() +sift = cv.SIFT_create() # find the keypoints and descriptors with SIFT kp1, des1 = sift.detectAndCompute(img1,None) @@ -93,14 +93,12 @@ search_params = dict(checks=50) flann = cv.FlannBasedMatcher(index_params,search_params) matches = flann.knnMatch(des1,des2,k=2) -good = [] pts1 = [] pts2 = [] # ratio test as per Lowe's paper for i,(m,n) in enumerate(matches): if m.distance < 0.8*n.distance: - good.append(m) pts2.append(kp2[m.trainIdx].pt) pts1.append(kp1[m.queryIdx].pt) @endcode From bb067c7ebff9e80c86a15777e33b552218ef23f0 Mon Sep 17 00:00:00 2001 From: Ian Maquignaz Date: Thu, 19 Nov 2020 06:20:20 -0500 Subject: [PATCH 4/6] Merge pull request #18849 from IanMaquignaz:fix_findFundamentalMat_parameters Minimum change to address issue #18837 --- modules/calib3d/include/opencv2/calib3d.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 7ce36c6fed..22c11cfac7 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -2153,10 +2153,10 @@ CV_EXPORTS void convertPointsHomogeneous( InputArray src, OutputArray dst ); floating-point (single or double precision). @param points2 Array of the second image points of the same size and format as points1 . @param method Method for computing a fundamental matrix. -- **CV_FM_7POINT** for a 7-point algorithm. \f$N = 7\f$ -- **CV_FM_8POINT** for an 8-point algorithm. \f$N \ge 8\f$ -- **CV_FM_RANSAC** for the RANSAC algorithm. \f$N \ge 8\f$ -- **CV_FM_LMEDS** for the LMedS algorithm. \f$N \ge 8\f$ +- @ref FM_7POINT for a 7-point algorithm. \f$N = 7\f$ +- @ref FM_8POINT for an 8-point algorithm. \f$N \ge 8\f$ +- @ref FM_RANSAC for the RANSAC algorithm. \f$N \ge 8\f$ +- @ref FM_LMEDS for the LMedS algorithm. \f$N \ge 8\f$ @param ransacReprojThreshold Parameter used only for RANSAC. It is the maximum distance from a point to an epipolar line in pixels, beyond which the point is considered an outlier and is not used for computing the final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the From ac24a72e669a7550516dc92304792e3800b6c85c Mon Sep 17 00:00:00 2001 From: Julien <182520+JulienMaille@users.noreply.github.com> Date: Fri, 20 Nov 2020 12:14:00 +0100 Subject: [PATCH 5/6] Merge pull request #18841 from JulienMaille:patch-2 Fixing dnn Resize layer for variable input size * Fix onnx loading of resize/upsample layers for different opset * group all DynamicResize tests * cleaned up scales checks * Simplify branching --- modules/dnn/src/onnx/onnx_importer.cpp | 78 +++++++++++++------------ modules/dnn/test/test_onnx_importer.cpp | 7 ++- 2 files changed, 48 insertions(+), 37 deletions(-) diff --git a/modules/dnn/src/onnx/onnx_importer.cpp b/modules/dnn/src/onnx/onnx_importer.cpp index 5289feef57..f5db2033ca 100644 --- a/modules/dnn/src/onnx/onnx_importer.cpp +++ b/modules/dnn/src/onnx/onnx_importer.cpp @@ -1746,43 +1746,45 @@ void ONNXImporter::handleNode(const opencv_onnx::NodeProto& node_proto_) for (int i = 1; i < node_proto.input_size(); i++) CV_Assert(layer_id.find(node_proto.input(i)) == layer_id.end()); - String interp_mode; if (layerParams.has("coordinate_transformation_mode")) - interp_mode = layerParams.get("coordinate_transformation_mode"); - else - interp_mode = layerParams.get("mode"); - CV_Assert_N(interp_mode != "tf_crop_and_resize", interp_mode != "tf_half_pixel_for_nn"); + { + String interp_mode = layerParams.get("coordinate_transformation_mode"); + CV_Assert_N(interp_mode != "tf_crop_and_resize", interp_mode != "tf_half_pixel_for_nn"); - layerParams.set("align_corners", interp_mode == "align_corners"); - Mat shapes = getBlob(node_proto, node_proto.input_size() - 1); - CV_CheckEQ(shapes.size[0], 4, ""); - CV_CheckEQ(shapes.size[1], 1, ""); - CV_CheckDepth(shapes.depth(), shapes.depth() == CV_32S || shapes.depth() == CV_32F, ""); - if (shapes.depth() == CV_32F) - shapes.convertTo(shapes, CV_32S); - int height = shapes.at(2); - int width = shapes.at(3); - if (hasDynamicShapes) - { - layerParams.set("zoom_factor_x", width); - layerParams.set("zoom_factor_y", height); - } - else - { - if (node_proto.input_size() == 3) { - IterShape_t shapeIt = outShapes.find(node_proto.input(0)); - CV_Assert(shapeIt != outShapes.end()); - MatShape scales = shapeIt->second; - height *= scales[2]; - width *= scales[3]; + layerParams.set("align_corners", interp_mode == "align_corners"); + if (layerParams.get("mode") == "linear") + { + layerParams.set("mode", interp_mode == "pytorch_half_pixel" ? + "opencv_linear" : "bilinear"); } - layerParams.set("width", width); - layerParams.set("height", height); } + if (layerParams.get("mode") == "linear" && framework_name == "pytorch") + layerParams.set("mode", "opencv_linear"); - if (layerParams.get("mode") == "linear") { - layerParams.set("mode", interp_mode == "pytorch_half_pixel" ? - "opencv_linear" : "bilinear"); + // input = [X, scales], [X, roi, scales] or [x, roi, scales, sizes] + int foundScaleId = hasDynamicShapes ? node_proto.input_size() - 1 + : node_proto.input_size() > 2 ? 2 : 1; + + Mat scales = getBlob(node_proto, foundScaleId); + if (scales.total() == 4) + { + layerParams.set("zoom_factor_y", scales.at(2)); + layerParams.set("zoom_factor_x", scales.at(3)); + } + else + { + const std::string& inputLast = node_proto.input(node_proto.input_size() - 1); + if (constBlobs.find(inputLast) != constBlobs.end()) + { + Mat shapes = getBlob(inputLast); + CV_CheckEQ(shapes.size[0], 4, ""); + CV_CheckEQ(shapes.size[1], 1, ""); + CV_CheckDepth(shapes.depth(), shapes.depth() == CV_32S || shapes.depth() == CV_32F, ""); + if (shapes.depth() == CV_32F) + shapes.convertTo(shapes, CV_32S); + layerParams.set("width", shapes.at(3)); + layerParams.set("height", shapes.at(2)); + } } replaceLayerParam(layerParams, "mode", "interpolation"); } @@ -1822,10 +1824,14 @@ void ONNXImporter::handleNode(const opencv_onnx::NodeProto& node_proto_) else { // scales as input - Mat scales = getBlob(node_proto, 1); - CV_Assert(scales.total() == 4); - layerParams.set("zoom_factor_y", scales.at(2)); - layerParams.set("zoom_factor_x", scales.at(3)); + const std::string& input1 = node_proto.input(1); + if (constBlobs.find(input1) != constBlobs.end()) + { + Mat scales = getBlob(input1); + CV_Assert(scales.total() == 4); + layerParams.set("zoom_factor_y", scales.at(2)); + layerParams.set("zoom_factor_x", scales.at(3)); + } } replaceLayerParam(layerParams, "mode", "interpolation"); } diff --git a/modules/dnn/test/test_onnx_importer.cpp b/modules/dnn/test/test_onnx_importer.cpp index 21be4fecb2..897b95ad8e 100644 --- a/modules/dnn/test/test_onnx_importer.cpp +++ b/modules/dnn/test/test_onnx_importer.cpp @@ -518,7 +518,12 @@ TEST_P(Test_ONNX_layers, Broadcast) TEST_P(Test_ONNX_layers, DynamicResize) { - testONNXModels("dynamic_resize", npy, 0, 0, false, true, 2); + testONNXModels("dynamic_resize_9", npy, 0, 0, false, true, 2); + testONNXModels("dynamic_resize_10", npy, 0, 0, false, true, 2); + testONNXModels("dynamic_resize_11", npy, 0, 0, false, true, 2); + testONNXModels("dynamic_resize_scale_9", npy, 0, 0, false, true, 2); + testONNXModels("dynamic_resize_scale_10", npy, 0, 0, false, true, 2); + testONNXModels("dynamic_resize_scale_11", npy, 0, 0, false, true, 2); } TEST_P(Test_ONNX_layers, Div) From 2255973b0f38591550ae08ed71dbc9056f317b96 Mon Sep 17 00:00:00 2001 From: Nathan Godwin Date: Fri, 20 Nov 2020 05:25:17 -0600 Subject: [PATCH 6/6] Merge pull request #18371 from nathanrgodwin:sqpnp_dev Added SQPnP algorithm to SolvePnP * Added sqpnp * Fixed test case * Added fix for duplicate point checking and inverse func reuse * Changes for 3x speedup Changed norm method (significant speed increase), changed nearest rotation computation to FOAM * Added symmetric 3x3 inverse and unrolled loops * Fixed error with SVD * Fixed error from with indices Indices were initialized negative. When nullspace is large, points coplanar, and rotation near 0, indices not changed. --- modules/calib3d/doc/calib3d.bib | 8 + modules/calib3d/include/opencv2/calib3d.hpp | 5 + modules/calib3d/src/solvepnp.cpp | 15 +- modules/calib3d/src/sqpnp.cpp | 775 ++++++++++++++++++ modules/calib3d/src/sqpnp.hpp | 194 +++++ modules/calib3d/test/test_solvepnp_ransac.cpp | 4 + 6 files changed, 999 insertions(+), 2 deletions(-) create mode 100644 modules/calib3d/src/sqpnp.cpp create mode 100644 modules/calib3d/src/sqpnp.hpp diff --git a/modules/calib3d/doc/calib3d.bib b/modules/calib3d/doc/calib3d.bib index 57989b34fd..a7e5a23982 100644 --- a/modules/calib3d/doc/calib3d.bib +++ b/modules/calib3d/doc/calib3d.bib @@ -39,3 +39,11 @@ year={2013}, publisher={IEEE} } + +@inproceedings{Terzakis20, + author = {Terzakis, George and Lourakis, Manolis}, + year = {2020}, + month = {09}, + pages = {}, + title = {A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem} +} diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 22c11cfac7..812c6be108 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -464,6 +464,7 @@ enum SolvePnPMethod { //!< - point 1: [ squareLength / 2, squareLength / 2, 0] //!< - point 2: [ squareLength / 2, -squareLength / 2, 0] //!< - point 3: [-squareLength / 2, -squareLength / 2, 0] + SOLVEPNP_SQPNP = 8, //!< SQPnP: A Consistently Fast and Globally OptimalSolution to the Perspective-n-Point Problem @cite Terzakis20 #ifndef CV_DOXYGEN SOLVEPNP_MAX_COUNT //!< Used for count #endif @@ -835,6 +836,9 @@ It requires 4 coplanar object points defined in the following order: - point 1: [ squareLength / 2, squareLength / 2, 0] - point 2: [ squareLength / 2, -squareLength / 2, 0] - point 3: [-squareLength / 2, -squareLength / 2, 0] +- **SOLVEPNP_SQPNP** Method is based on the paper "A Consistently Fast and Globally Optimal Solution to the +Perspective-n-Point Problem" by G. Terzakis and M.Lourakis (@cite Terzakis20). It requires 3 or more points. + The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera intrinsic matrix and the distortion coefficients, see the figure below @@ -958,6 +962,7 @@ a 3D point expressed in the world frame into the camera frame: - point 1: [ squareLength / 2, squareLength / 2, 0] - point 2: [ squareLength / 2, -squareLength / 2, 0] - point 3: [-squareLength / 2, -squareLength / 2, 0] + - With **SOLVEPNP_SQPNP** input points must be >= 3 */ CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 0e3a1e8f22..cac04c4869 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -47,6 +47,7 @@ #include "p3p.h" #include "ap3p.h" #include "ippe.hpp" +#include "sqpnp.hpp" #include "opencv2/calib3d/calib3d_c.h" #include @@ -751,7 +752,8 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) + CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) + || (npoints >= 3 && flags == SOLVEPNP_SQPNP) ) && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); opoints = opoints.reshape(3, npoints); @@ -936,6 +938,14 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, } } catch (...) { } } + else if (flags == SOLVEPNP_SQPNP) + { + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + + sqpnp::PoseSolver solver; + solver.solve(opoints, undistortedPoints, vec_rvecs, vec_tvecs); + } /*else if (flags == SOLVEPNP_DLS) { Mat undistortedPoints; @@ -963,7 +973,8 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, vec_tvecs.push_back(tvec); }*/ else - CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); + CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, " + "SOLVEPNP_EPNP, SOLVEPNP_DLS, SOLVEPNP_UPNP, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE or SOLVEPNP_SQPNP"); CV_Assert(vec_rvecs.size() == vec_tvecs.size()); diff --git a/modules/calib3d/src/sqpnp.cpp b/modules/calib3d/src/sqpnp.cpp new file mode 100644 index 0000000000..10ea96c423 --- /dev/null +++ b/modules/calib3d/src/sqpnp.cpp @@ -0,0 +1,775 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This file is based on file issued with the following license: + +/* +BSD 3-Clause License + +Copyright (c) 2020, George Terzakis +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "precomp.hpp" +#include "sqpnp.hpp" + +#include + +namespace cv { +namespace sqpnp { + +const double PoseSolver::RANK_TOLERANCE = 1e-7; +const double PoseSolver::SQP_SQUARED_TOLERANCE = 1e-10; +const double PoseSolver::SQP_DET_THRESHOLD = 1.001; +const double PoseSolver::ORTHOGONALITY_SQUARED_ERROR_THRESHOLD = 1e-8; +const double PoseSolver::EQUAL_VECTORS_SQUARED_DIFF = 1e-10; +const double PoseSolver::EQUAL_SQUARED_ERRORS_DIFF = 1e-6; +const double PoseSolver::POINT_VARIANCE_THRESHOLD = 1e-5; +const double PoseSolver::SQRT3 = std::sqrt(3); +const int PoseSolver::SQP_MAX_ITERATION = 15; + +//No checking done here for overflow, since this is not public all call instances +//are assumed to be valid +template + void set(int row, int col, cv::Matx& dest, + const cv::Matx& source) +{ + for (int y = 0; y < snrows; y++) + { + for (int x = 0; x < sncols; x++) + { + dest(row + y, col + x) = source(y, x); + } + } +} + +PoseSolver::PoseSolver() + : num_null_vectors_(-1), + num_solutions_(0) +{ +} + + +void PoseSolver::solve(InputArray objectPoints, InputArray imagePoints, OutputArrayOfArrays rvecs, + OutputArrayOfArrays tvecs) +{ + //Input checking + int objType = objectPoints.getMat().type(); + CV_CheckType(objType, objType == CV_32FC3 || objType == CV_64FC3, + "Type of objectPoints must be CV_32FC3 or CV_64FC3"); + + int imgType = imagePoints.getMat().type(); + CV_CheckType(imgType, imgType == CV_32FC2 || imgType == CV_64FC2, + "Type of imagePoints must be CV_32FC2 or CV_64FC2"); + + CV_Assert(objectPoints.rows() == 1 || objectPoints.cols() == 1); + CV_Assert(objectPoints.rows() >= 3 || objectPoints.cols() >= 3); + CV_Assert(imagePoints.rows() == 1 || imagePoints.cols() == 1); + CV_Assert(imagePoints.rows() * imagePoints.cols() == objectPoints.rows() * objectPoints.cols()); + + Mat _imagePoints; + if (imgType == CV_32FC2) + { + imagePoints.getMat().convertTo(_imagePoints, CV_64F); + } + else + { + _imagePoints = imagePoints.getMat(); + } + + Mat _objectPoints; + if (objType == CV_32FC3) + { + objectPoints.getMat().convertTo(_objectPoints, CV_64F); + } + else + { + _objectPoints = objectPoints.getMat(); + } + + num_null_vectors_ = -1; + num_solutions_ = 0; + + computeOmega(_objectPoints, _imagePoints); + solveInternal(); + + int depthRot = rvecs.fixedType() ? rvecs.depth() : CV_64F; + int depthTrans = tvecs.fixedType() ? tvecs.depth() : CV_64F; + + rvecs.create(num_solutions_, 1, CV_MAKETYPE(depthRot, rvecs.fixedType() && rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); + tvecs.create(num_solutions_, 1, CV_MAKETYPE(depthTrans, tvecs.fixedType() && tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); + + for (int i = 0; i < num_solutions_; i++) + { + + Mat rvec; + Mat rotation = Mat(solutions_[i].r_hat).reshape(1, 3); + Rodrigues(rotation, rvec); + + rvecs.getMatRef(i) = rvec; + tvecs.getMatRef(i) = Mat(solutions_[i].t); + } +} + +void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) +{ + omega_ = cv::Matx::zeros(); + cv::Matx qa_sum = cv::Matx::zeros(); + + cv::Point2d sum_img(0, 0); + cv::Point3d sum_obj(0, 0, 0); + double sq_norm_sum = 0; + + Mat _imagePoints = imagePoints.getMat(); + Mat _objectPoints = objectPoints.getMat(); + + int n = _objectPoints.cols * _objectPoints.rows; + + for (int i = 0; i < n; i++) + { + const cv::Point2d& img_pt = _imagePoints.at(i); + const cv::Point3d& obj_pt = _objectPoints.at(i); + + sum_img += img_pt; + sum_obj += obj_pt; + + const double& x = img_pt.x, & y = img_pt.y; + const double& X = obj_pt.x, & Y = obj_pt.y, & Z = obj_pt.z; + double sq_norm = x * x + y * y; + sq_norm_sum += sq_norm; + + double X2 = X * X, + XY = X * Y, + XZ = X * Z, + Y2 = Y * Y, + YZ = Y * Z, + Z2 = Z * Z; + + omega_(0, 0) += X2; + omega_(0, 1) += XY; + omega_(0, 2) += XZ; + omega_(1, 1) += Y2; + omega_(1, 2) += YZ; + omega_(2, 2) += Z2; + + + //Populating this manually saves operations by only calculating upper triangle + omega_(0, 6) += -x * X2; omega_(0, 7) += -x * XY; omega_(0, 8) += -x * XZ; + omega_(1, 7) += -x * Y2; omega_(1, 8) += -x * YZ; + omega_(2, 8) += -x * Z2; + + omega_(3, 6) += -y * X2; omega_(3, 7) += -y * XY; omega_(3, 8) += -y * XZ; + omega_(4, 7) += -y * Y2; omega_(4, 8) += -y * YZ; + omega_(5, 8) += -y * Z2; + + + omega_(6, 6) += sq_norm * X2; omega_(6, 7) += sq_norm * XY; omega_(6, 8) += sq_norm * XZ; + omega_(7, 7) += sq_norm * Y2; omega_(7, 8) += sq_norm * YZ; + omega_(8, 8) += sq_norm * Z2; + + //Compute qa_sum + qa_sum(0, 0) += X; qa_sum(0, 1) += Y; qa_sum(0, 2) += Z; + qa_sum(1, 3) += X; qa_sum(1, 4) += Y; qa_sum(1, 5) += Z; + + qa_sum(0, 6) += -x * X; qa_sum(0, 7) += -x * Y; qa_sum(0, 8) += -x * Z; + qa_sum(1, 6) += -y * X; qa_sum(1, 7) += -y * Y; qa_sum(1, 8) += -y * Z; + + qa_sum(2, 0) += -x * X; qa_sum(2, 1) += -x * Y; qa_sum(2, 2) += -x * Z; + qa_sum(2, 3) += -y * X; qa_sum(2, 4) += -y * Y; qa_sum(2, 5) += -y * Z; + + qa_sum(2, 6) += sq_norm * X; qa_sum(2, 7) += sq_norm * Y; qa_sum(2, 8) += sq_norm * Z; + } + + + omega_(1, 6) = omega_(0, 7); omega_(2, 6) = omega_(0, 8); omega_(2, 7) = omega_(1, 8); + omega_(4, 6) = omega_(3, 7); omega_(5, 6) = omega_(3, 8); omega_(5, 7) = omega_(4, 8); + omega_(7, 6) = omega_(6, 7); omega_(8, 6) = omega_(6, 8); omega_(8, 7) = omega_(7, 8); + + + omega_(3, 3) = omega_(0, 0); omega_(3, 4) = omega_(0, 1); omega_(3, 5) = omega_(0, 2); + omega_(4, 4) = omega_(1, 1); omega_(4, 5) = omega_(1, 2); + omega_(5, 5) = omega_(2, 2); + + //Mirror upper triangle to lower triangle + for (int r = 0; r < 9; r++) + { + for (int c = 0; c < r; c++) + { + omega_(r, c) = omega_(c, r); + } + } + + cv::Matx q; + q(0, 0) = n; q(0, 1) = 0; q(0, 2) = -sum_img.x; + q(1, 0) = 0; q(1, 1) = n; q(1, 2) = -sum_img.y; + q(2, 0) = -sum_img.x; q(2, 1) = -sum_img.y; q(2, 2) = sq_norm_sum; + + double inv_n = 1.0 / n; + double detQ = n * (n * sq_norm_sum - sum_img.y * sum_img.y - sum_img.x * sum_img.x); + double point_coordinate_variance = detQ * inv_n * inv_n * inv_n; + + CV_Assert(point_coordinate_variance >= POINT_VARIANCE_THRESHOLD); + + Matx q_inv; + analyticalInverse3x3Symm(q, q_inv); + + p_ = -q_inv * qa_sum; + + omega_ += qa_sum.t() * p_; + + cv::SVD omega_svd(omega_, cv::SVD::FULL_UV); + s_ = omega_svd.w; + u_ = cv::Mat(omega_svd.vt.t()); + + CV_Assert(s_(0) >= 1e-7); + + while (s_(7 - num_null_vectors_) < RANK_TOLERANCE) num_null_vectors_++; + + CV_Assert(++num_null_vectors_ <= 6); + + point_mean_ = cv::Vec3d(sum_obj.x / n, sum_obj.y / n, sum_obj.z / n); +} + +void PoseSolver::solveInternal() +{ + double min_sq_err = std::numeric_limits::max(); + int num_eigen_points = num_null_vectors_ > 0 ? num_null_vectors_ : 1; + + for (int i = 9 - num_eigen_points; i < 9; i++) + { + const cv::Matx e = SQRT3 * u_.col(i); + double orthogonality_sq_err = orthogonalityError(e); + + SQPSolution solutions[2]; + + //If e is orthogonal, we can skip SQP + if (orthogonality_sq_err < ORTHOGONALITY_SQUARED_ERROR_THRESHOLD) + { + solutions[0].r_hat = det3x3(e) * e; + solutions[0].t = p_ * solutions[0].r_hat; + checkSolution(solutions[0], min_sq_err); + } + else + { + Matx r; + nearestRotationMatrix(e, r); + solutions[0] = runSQP(r); + solutions[0].t = p_ * solutions[0].r_hat; + checkSolution(solutions[0], min_sq_err); + + nearestRotationMatrix(-e, r); + solutions[1] = runSQP(r); + solutions[1].t = p_ * solutions[1].r_hat; + checkSolution(solutions[1], min_sq_err); + } + } + + int c = 1; + + while (min_sq_err > 3 * s_[9 - num_eigen_points - c] && 9 - num_eigen_points - c > 0) + { + int index = 9 - num_eigen_points - c; + + const cv::Matx e = u_.col(index); + SQPSolution solutions[2]; + + Matx r; + nearestRotationMatrix(e, r); + solutions[0] = runSQP(r); + solutions[0].t = p_ * solutions[0].r_hat; + checkSolution(solutions[0], min_sq_err); + + nearestRotationMatrix(-e, r); + solutions[1] = runSQP(r); + solutions[1].t = p_ * solutions[1].r_hat; + checkSolution(solutions[1], min_sq_err); + + c++; + } +} + +PoseSolver::SQPSolution PoseSolver::runSQP(const cv::Matx& r0) +{ + cv::Matx r = r0; + + double delta_squared_norm = std::numeric_limits::max(); + cv::Matx delta; + + int step = 0; + while (delta_squared_norm > SQP_SQUARED_TOLERANCE && step++ < SQP_MAX_ITERATION) + { + solveSQPSystem(r, delta); + r += delta; + delta_squared_norm = cv::norm(delta, cv::NORM_L2SQR); + } + + SQPSolution solution; + + double det_r = det3x3(r); + if (det_r < 0) + { + r = -r; + det_r = -det_r; + } + + if (det_r > SQP_DET_THRESHOLD) + { + nearestRotationMatrix(r, solution.r_hat); + } + else + { + solution.r_hat = r; + } + + return solution; +} + +void PoseSolver::solveSQPSystem(const cv::Matx& r, cv::Matx& delta) +{ + double sqnorm_r1 = r(0) * r(0) + r(1) * r(1) + r(2) * r(2), + sqnorm_r2 = r(3) * r(3) + r(4) * r(4) + r(5) * r(5), + sqnorm_r3 = r(6) * r(6) + r(7) * r(7) + r(8) * r(8); + double dot_r1r2 = r(0) * r(3) + r(1) * r(4) + r(2) * r(5), + dot_r1r3 = r(0) * r(6) + r(1) * r(7) + r(2) * r(8), + dot_r2r3 = r(3) * r(6) + r(4) * r(7) + r(5) * r(8); + + cv::Matx N; + cv::Matx H; + cv::Matx JH; + + computeRowAndNullspace(r, H, N, JH); + + cv::Matx g; + g(0) = 1 - sqnorm_r1; g(1) = 1 - sqnorm_r2; g(2) = 1 - sqnorm_r3; g(3) = -dot_r1r2; g(4) = -dot_r2r3; g(5) = -dot_r1r3; + + cv::Matx x; + x(0) = g(0) / JH(0, 0); + x(1) = g(1) / JH(1, 1); + x(2) = g(2) / JH(2, 2); + x(3) = (g(3) - JH(3, 0) * x(0) - JH(3, 1) * x(1)) / JH(3, 3); + x(4) = (g(4) - JH(4, 1) * x(1) - JH(4, 2) * x(2) - JH(4, 3) * x(3)) / JH(4, 4); + x(5) = (g(5) - JH(5, 0) * x(0) - JH(5, 2) * x(2) - JH(5, 3) * x(3) - JH(5, 4) * x(4)) / JH(5, 5); + + delta = H * x; + + + cv::Matx nt_omega = N.t() * omega_; + cv::Matx W = nt_omega * N, W_inv; + + analyticalInverse3x3Symm(W, W_inv); + + cv::Matx y = -W_inv * nt_omega * (delta + r); + delta += N * y; +} + +bool PoseSolver::analyticalInverse3x3Symm(const cv::Matx& Q, + cv::Matx& Qinv, + const double& threshold) +{ + // 1. Get the elements of the matrix + double a = Q(0, 0), + b = Q(1, 0), d = Q(1, 1), + c = Q(2, 0), e = Q(2, 1), f = Q(2, 2); + + // 2. Determinant + double t2, t4, t7, t9, t12; + t2 = e * e; + t4 = a * d; + t7 = b * b; + t9 = b * c; + t12 = c * c; + double det = -t4 * f + a * t2 + t7 * f - 2.0 * t9 * e + t12 * d; + + if (fabs(det) < threshold) return false; + + // 3. Inverse + double t15, t20, t24, t30; + t15 = 1.0 / det; + t20 = (-b * f + c * e) * t15; + t24 = (b * e - c * d) * t15; + t30 = (a * e - t9) * t15; + Qinv(0, 0) = (-d * f + t2) * t15; + Qinv(0, 1) = Qinv(1, 0) = -t20; + Qinv(0, 2) = Qinv(2, 0) = -t24; + Qinv(1, 1) = -(a * f - t12) * t15; + Qinv(1, 2) = Qinv(2, 1) = t30; + Qinv(2, 2) = -(t4 - t7) * t15; + + return true; +} + +void PoseSolver::computeRowAndNullspace(const cv::Matx& r, + cv::Matx& H, + cv::Matx& N, + cv::Matx& K, + const double& norm_threshold) +{ + H = cv::Matx::zeros(); + + // 1. q1 + double norm_r1 = sqrt(r(0) * r(0) + r(1) * r(1) + r(2) * r(2)); + double inv_norm_r1 = norm_r1 > 1e-5 ? 1.0 / norm_r1 : 0.0; + H(0, 0) = r(0) * inv_norm_r1; + H(1, 0) = r(1) * inv_norm_r1; + H(2, 0) = r(2) * inv_norm_r1; + K(0, 0) = 2 * norm_r1; + + // 2. q2 + double norm_r2 = sqrt(r(3) * r(3) + r(4) * r(4) + r(5) * r(5)); + double inv_norm_r2 = 1.0 / norm_r2; + H(3, 1) = r(3) * inv_norm_r2; + H(4, 1) = r(4) * inv_norm_r2; + H(5, 1) = r(5) * inv_norm_r2; + K(1, 0) = 0; + K(1, 1) = 2 * norm_r2; + + // 3. q3 = (r3'*q2)*q2 - (r3'*q1)*q1 ; q3 = q3/norm(q3) + double norm_r3 = sqrt(r(6) * r(6) + r(7) * r(7) + r(8) * r(8)); + double inv_norm_r3 = 1.0 / norm_r3; + H(6, 2) = r(6) * inv_norm_r3; + H(7, 2) = r(7) * inv_norm_r3; + H(8, 2) = r(8) * inv_norm_r3; + K(2, 0) = K(2, 1) = 0; + K(2, 2) = 2 * norm_r3; + + // 4. q4 + double dot_j4q1 = r(3) * H(0, 0) + r(4) * H(1, 0) + r(5) * H(2, 0), + dot_j4q2 = r(0) * H(3, 1) + r(1) * H(4, 1) + r(2) * H(5, 1); + + H(0, 3) = r(3) - dot_j4q1 * H(0, 0); + H(1, 3) = r(4) - dot_j4q1 * H(1, 0); + H(2, 3) = r(5) - dot_j4q1 * H(2, 0); + H(3, 3) = r(0) - dot_j4q2 * H(3, 1); + H(4, 3) = r(1) - dot_j4q2 * H(4, 1); + H(5, 3) = r(2) - dot_j4q2 * H(5, 1); + double inv_norm_j4 = 1.0 / sqrt(H(0, 3) * H(0, 3) + H(1, 3) * H(1, 3) + H(2, 3) * H(2, 3) + + H(3, 3) * H(3, 3) + H(4, 3) * H(4, 3) + H(5, 3) * H(5, 3)); + + H(0, 3) *= inv_norm_j4; + H(1, 3) *= inv_norm_j4; + H(2, 3) *= inv_norm_j4; + H(3, 3) *= inv_norm_j4; + H(4, 3) *= inv_norm_j4; + H(5, 3) *= inv_norm_j4; + + K(3, 0) = r(3) * H(0, 0) + r(4) * H(1, 0) + r(5) * H(2, 0); + K(3, 1) = r(0) * H(3, 1) + r(1) * H(4, 1) + r(2) * H(5, 1); + K(3, 2) = 0; + K(3, 3) = r(3) * H(0, 3) + r(4) * H(1, 3) + r(5) * H(2, 3) + r(0) * H(3, 3) + r(1) * H(4, 3) + r(2) * H(5, 3); + + // 5. q5 + double dot_j5q2 = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1); + double dot_j5q3 = r(3) * H(6, 2) + r(4) * H(7, 2) + r(5) * H(8, 2); + double dot_j5q4 = r(6) * H(3, 3) + r(7) * H(4, 3) + r(8) * H(5, 3); + + H(0, 4) = -dot_j5q4 * H(0, 3); + H(1, 4) = -dot_j5q4 * H(1, 3); + H(2, 4) = -dot_j5q4 * H(2, 3); + H(3, 4) = r(6) - dot_j5q2 * H(3, 1) - dot_j5q4 * H(3, 3); + H(4, 4) = r(7) - dot_j5q2 * H(4, 1) - dot_j5q4 * H(4, 3); + H(5, 4) = r(8) - dot_j5q2 * H(5, 1) - dot_j5q4 * H(5, 3); + H(6, 4) = r(3) - dot_j5q3 * H(6, 2); H(7, 4) = r(4) - dot_j5q3 * H(7, 2); H(8, 4) = r(5) - dot_j5q3 * H(8, 2); + + Matx q4 = H.col(4); + q4 /= cv::norm(q4); + set(0, 4, H, q4); + + K(4, 0) = 0; + K(4, 1) = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1); + K(4, 2) = r(3) * H(6, 2) + r(4) * H(7, 2) + r(5) * H(8, 2); + K(4, 3) = r(6) * H(3, 3) + r(7) * H(4, 3) + r(8) * H(5, 3); + K(4, 4) = r(6) * H(3, 4) + r(7) * H(4, 4) + r(8) * H(5, 4) + r(3) * H(6, 4) + r(4) * H(7, 4) + r(5) * H(8, 4); + + + // 4. q6 + double dot_j6q1 = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0); + double dot_j6q3 = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2); + double dot_j6q4 = r(6) * H(0, 3) + r(7) * H(1, 3) + r(8) * H(2, 3); + double dot_j6q5 = r(0) * H(6, 4) + r(1) * H(7, 4) + r(2) * H(8, 4) + r(6) * H(0, 4) + r(7) * H(1, 4) + r(8) * H(2, 4); + + H(0, 5) = r(6) - dot_j6q1 * H(0, 0) - dot_j6q4 * H(0, 3) - dot_j6q5 * H(0, 4); + H(1, 5) = r(7) - dot_j6q1 * H(1, 0) - dot_j6q4 * H(1, 3) - dot_j6q5 * H(1, 4); + H(2, 5) = r(8) - dot_j6q1 * H(2, 0) - dot_j6q4 * H(2, 3) - dot_j6q5 * H(2, 4); + + H(3, 5) = -dot_j6q5 * H(3, 4) - dot_j6q4 * H(3, 3); + H(4, 5) = -dot_j6q5 * H(4, 4) - dot_j6q4 * H(4, 3); + H(5, 5) = -dot_j6q5 * H(5, 4) - dot_j6q4 * H(5, 3); + + H(6, 5) = r(0) - dot_j6q3 * H(6, 2) - dot_j6q5 * H(6, 4); + H(7, 5) = r(1) - dot_j6q3 * H(7, 2) - dot_j6q5 * H(7, 4); + H(8, 5) = r(2) - dot_j6q3 * H(8, 2) - dot_j6q5 * H(8, 4); + + Matx q5 = H.col(5); + q5 /= cv::norm(q5); + set(0, 5, H, q5); + + K(5, 0) = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0); + K(5, 1) = 0; K(5, 2) = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2); + K(5, 3) = r(6) * H(0, 3) + r(7) * H(1, 3) + r(8) * H(2, 3); + K(5, 4) = r(6) * H(0, 4) + r(7) * H(1, 4) + r(8) * H(2, 4) + r(0) * H(6, 4) + r(1) * H(7, 4) + r(2) * H(8, 4); + K(5, 5) = r(6) * H(0, 5) + r(7) * H(1, 5) + r(8) * H(2, 5) + r(0) * H(6, 5) + r(1) * H(7, 5) + r(2) * H(8, 5); + + // Great! Now H is an orthogonalized, sparse basis of the Jacobian row space and K is filled. + // + // Now get a projector onto the null space H: + const cv::Matx Pn = cv::Matx::eye() - (H * H.t()); + + // Now we need to pick 3 columns of P with non-zero norm (> 0.3) and some angle between them (> 0.3). + // + // Find the 3 columns of Pn with largest norms + int index1 = 0, + index2 = 0, + index3 = 0; + double max_norm1 = std::numeric_limits::min(); + double min_dot12 = std::numeric_limits::max(); + double min_dot1323 = std::numeric_limits::max(); + + + double col_norms[9]; + for (int i = 0; i < 9; i++) + { + col_norms[i] = cv::norm(Pn.col(i)); + if (col_norms[i] >= norm_threshold) + { + if (max_norm1 < col_norms[i]) + { + max_norm1 = col_norms[i]; + index1 = i; + } + } + } + + Matx v1 = Pn.col(index1); + v1 /= max_norm1; + set(0, 0, N, v1); + + for (int i = 0; i < 9; i++) + { + if (i == index1) continue; + if (col_norms[i] >= norm_threshold) + { + double cos_v1_x_col = fabs(Pn.col(i).dot(v1) / col_norms[i]); + + if (cos_v1_x_col <= min_dot12) + { + index2 = i; + min_dot12 = cos_v1_x_col; + } + } + } + + Matx v2 = Pn.col(index2); + Matx n0 = N.col(0); + v2 -= v2.dot(n0) * n0; + v2 /= cv::norm(v2); + set(0, 1, N, v2); + + for (int i = 0; i < 9; i++) + { + if (i == index2 || i == index1) continue; + if (col_norms[i] >= norm_threshold) + { + double cos_v1_x_col = fabs(Pn.col(i).dot(v1) / col_norms[i]); + double cos_v2_x_col = fabs(Pn.col(i).dot(v2) / col_norms[i]); + + if (cos_v1_x_col + cos_v2_x_col <= min_dot1323) + { + index3 = i; + min_dot1323 = cos_v2_x_col + cos_v2_x_col; + } + } + } + + Matx v3 = Pn.col(index3); + Matx n1 = N.col(1); + v3 -= (v3.dot(n1)) * n1 - (v3.dot(n0)) * n0; + v3 /= cv::norm(v3); + set(0, 2, N, v3); + +} + +// faster nearest rotation computation based on FOAM (see: http://users.ics.forth.gr/~lourakis/publ/2018_iros.pdf ) +/* Solve the nearest orthogonal approximation problem + * i.e., given e, find R minimizing ||R-e||_F + * + * The computation borrows from Markley's FOAM algorithm + * "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. Astronaut. Sci. + * + * See also M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016 + * + * Copyright (C) 2019 Manolis Lourakis (lourakis **at** ics forth gr) + * Institute of Computer Science, Foundation for Research & Technology - Hellas + * Heraklion, Crete, Greece. + */ +void PoseSolver::nearestRotationMatrix(const cv::Matx& e, + cv::Matx& r) +{ + register int i; + double l, lprev, det_e, e_sq, adj_e_sq, adj_e[9]; + + // e's adjoint + adj_e[0] = e(4) * e(8) - e(5) * e(7); adj_e[1] = e(2) * e(7) - e(1) * e(8); adj_e[2] = e(1) * e(5) - e(2) * e(4); + adj_e[3] = e(5) * e(6) - e(3) * e(8); adj_e[4] = e(0) * e(8) - e(2) * e(6); adj_e[5] = e(2) * e(3) - e(0) * e(5); + adj_e[6] = e(3) * e(7) - e(4) * e(6); adj_e[7] = e(1) * e(6) - e(0) * e(7); adj_e[8] = e(0) * e(4) - e(1) * e(3); + + // det(e), ||e||^2, ||adj(e)||^2 + det_e = e(0) * e(4) * e(8) - e(0) * e(5) * e(7) - e(1) * e(3) * e(8) + e(2) * e(3) * e(7) + e(1) * e(6) * e(5) - e(2) * e(6) * e(4); + e_sq = e(0) * e(0) + e(1) * e(1) + e(2) * e(2) + e(3) * e(3) + e(4) * e(4) + e(5) * e(5) + e(6) * e(6) + e(7) * e(7) + e(8) * e(8); + adj_e_sq = adj_e[0] * adj_e[0] + adj_e[1] * adj_e[1] + adj_e[2] * adj_e[2] + adj_e[3] * adj_e[3] + adj_e[4] * adj_e[4] + adj_e[5] * adj_e[5] + adj_e[6] * adj_e[6] + adj_e[7] * adj_e[7] + adj_e[8] * adj_e[8]; + + // compute l_max with Newton-Raphson from FOAM's characteristic polynomial, i.e. eq.(23) - (26) + for (i = 200, l = 2.0, lprev = 0.0; fabs(l - lprev) > 1E-12 * fabs(lprev) && i > 0; --i) { + double tmp, p, pp; + + tmp = (l * l - e_sq); + p = (tmp * tmp - 8.0 * l * det_e - 4.0 * adj_e_sq); + pp = 8.0 * (0.5 * tmp * l - det_e); + + lprev = l; + l -= p / pp; + } + + // the rotation matrix equals ((l^2 + e_sq)*e + 2*l*adj(e') - 2*e*e'*e) / (l*(l*l-e_sq) - 2*det(e)), i.e. eq.(14) using (18), (19) + { + // compute (l^2 + e_sq)*e + double tmp[9], e_et[9], denom; + const double a = l * l + e_sq; + + // e_et=e*e' + e_et[0] = e(0) * e(0) + e(1) * e(1) + e(2) * e(2); + e_et[1] = e(0) * e(3) + e(1) * e(4) + e(2) * e(5); + e_et[2] = e(0) * e(6) + e(1) * e(7) + e(2) * e(8); + + e_et[3] = e_et[1]; + e_et[4] = e(3) * e(3) + e(4) * e(4) + e(5) * e(5); + e_et[5] = e(3) * e(6) + e(4) * e(7) + e(5) * e(8); + + e_et[6] = e_et[2]; + e_et[7] = e_et[5]; + e_et[8] = e(6) * e(6) + e(7) * e(7) + e(8) * e(8); + + // tmp=e_et*e + tmp[0] = e_et[0] * e(0) + e_et[1] * e(3) + e_et[2] * e(6); + tmp[1] = e_et[0] * e(1) + e_et[1] * e(4) + e_et[2] * e(7); + tmp[2] = e_et[0] * e(2) + e_et[1] * e(5) + e_et[2] * e(8); + + tmp[3] = e_et[3] * e(0) + e_et[4] * e(3) + e_et[5] * e(6); + tmp[4] = e_et[3] * e(1) + e_et[4] * e(4) + e_et[5] * e(7); + tmp[5] = e_et[3] * e(2) + e_et[4] * e(5) + e_et[5] * e(8); + + tmp[6] = e_et[6] * e(0) + e_et[7] * e(3) + e_et[8] * e(6); + tmp[7] = e_et[6] * e(1) + e_et[7] * e(4) + e_et[8] * e(7); + tmp[8] = e_et[6] * e(2) + e_et[7] * e(5) + e_et[8] * e(8); + + // compute R as (a*e + 2*(l*adj(e)' - tmp))*denom; note that adj(e')=adj(e)' + denom = l * (l * l - e_sq) - 2.0 * det_e; + denom = 1.0 / denom; + r(0) = (a * e(0) + 2.0 * (l * adj_e[0] - tmp[0])) * denom; + r(1) = (a * e(1) + 2.0 * (l * adj_e[3] - tmp[1])) * denom; + r(2) = (a * e(2) + 2.0 * (l * adj_e[6] - tmp[2])) * denom; + + r(3) = (a * e(3) + 2.0 * (l * adj_e[1] - tmp[3])) * denom; + r(4) = (a * e(4) + 2.0 * (l * adj_e[4] - tmp[4])) * denom; + r(5) = (a * e(5) + 2.0 * (l * adj_e[7] - tmp[5])) * denom; + + r(6) = (a * e(6) + 2.0 * (l * adj_e[2] - tmp[6])) * denom; + r(7) = (a * e(7) + 2.0 * (l * adj_e[5] - tmp[7])) * denom; + r(8) = (a * e(8) + 2.0 * (l * adj_e[8] - tmp[8])) * denom; + } +} + +double PoseSolver::det3x3(const cv::Matx& e) +{ + return e(0) * e(4) * e(8) + e(1) * e(5) * e(6) + e(2) * e(3) * e(7) + - e(6) * e(4) * e(2) - e(7) * e(5) * e(0) - e(8) * e(3) * e(1); +} + +inline bool PoseSolver::positiveDepth(const SQPSolution& solution) const +{ + const cv::Matx& r = solution.r_hat; + const cv::Matx& t = solution.t; + const cv::Vec3d& mean = point_mean_; + return (r(6) * mean(0) + r(7) * mean(1) + r(8) * mean(2) + t(2) > 0); +} + +void PoseSolver::checkSolution(SQPSolution& solution, double& min_error) +{ + if (positiveDepth(solution)) + { + solution.sq_error = (omega_ * solution.r_hat).ddot(solution.r_hat); + if (fabs(min_error - solution.sq_error) > EQUAL_SQUARED_ERRORS_DIFF) + { + if (min_error > solution.sq_error) + { + min_error = solution.sq_error; + solutions_[0] = solution; + num_solutions_ = 1; + } + } + else + { + bool found = false; + for (int i = 0; i < num_solutions_; i++) + { + if (cv::norm(solutions_[i].r_hat - solution.r_hat, cv::NORM_L2SQR) < EQUAL_VECTORS_SQUARED_DIFF) + { + if (solutions_[i].sq_error > solution.sq_error) + { + solutions_[i] = solution; + } + found = true; + break; + } + } + + if (!found) + { + solutions_[num_solutions_++] = solution; + } + if (min_error > solution.sq_error) min_error = solution.sq_error; + } + } +} + +double PoseSolver::orthogonalityError(const cv::Matx& e) +{ + double sq_norm_e1 = e(0) * e(0) + e(1) * e(1) + e(2) * e(2); + double sq_norm_e2 = e(3) * e(3) + e(4) * e(4) + e(5) * e(5); + double sq_norm_e3 = e(6) * e(6) + e(7) * e(7) + e(8) * e(8); + double dot_e1e2 = e(0) * e(3) + e(1) * e(4) + e(2) * e(5); + double dot_e1e3 = e(0) * e(6) + e(1) * e(7) + e(2) * e(8); + double dot_e2e3 = e(3) * e(6) + e(4) * e(7) + e(5) * e(8); + + return (sq_norm_e1 - 1) * (sq_norm_e1 - 1) + (sq_norm_e2 - 1) * (sq_norm_e2 - 1) + (sq_norm_e3 - 1) * (sq_norm_e3 - 1) + + 2 * (dot_e1e2 * dot_e1e2 + dot_e1e3 * dot_e1e3 + dot_e2e3 * dot_e2e3); +} + +} +} diff --git a/modules/calib3d/src/sqpnp.hpp b/modules/calib3d/src/sqpnp.hpp new file mode 100644 index 0000000000..f8136324c9 --- /dev/null +++ b/modules/calib3d/src/sqpnp.hpp @@ -0,0 +1,194 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This file is based on file issued with the following license: + +/* +BSD 3-Clause License + +Copyright (c) 2020, George Terzakis +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef OPENCV_CALIB3D_SQPNP_HPP +#define OPENCV_CALIB3D_SQPNP_HPP + +#include + +namespace cv { +namespace sqpnp { + + +class PoseSolver { +public: + /** + * @brief PoseSolver constructor + */ + PoseSolver(); + + /** + * @brief Finds the possible poses of a camera given a set of 3D points + * and their corresponding 2D image projections. The poses are + * sorted by lowest squared error (which corresponds to lowest + * reprojection error). + * @param objectPoints Array or vector of 3 or more 3D points defined in object coordinates. + * 1xN/Nx1 3-channel (float or double) where N is the number of points. + * @param imagePoints Array or vector of corresponding 2D points, 1xN/Nx1 2-channel. + * @param rvec The output rotation solutions (up to 18 3x1 rotation vectors) + * @param tvec The output translation solutions (up to 18 3x1 vectors) + */ + void solve(InputArray objectPoints, InputArray imagePoints, OutputArrayOfArrays rvec, + OutputArrayOfArrays tvec); + +private: + struct SQPSolution + { + cv::Matx r_hat; + cv::Matx t; + double sq_error; + }; + + /* + * @brief Computes the 9x9 PSD Omega matrix and supporting matrices. + * @param objectPoints Array or vector of 3 or more 3D points defined in object coordinates. + * 1xN/Nx1 3-channel (float or double) where N is the number of points. + * @param imagePoints Array or vector of corresponding 2D points, 1xN/Nx1 2-channel. + */ + void computeOmega(InputArray objectPoints, InputArray imagePoints); + + /* + * @brief Computes the 9x9 PSD Omega matrix and supporting matrices. + */ + void solveInternal(); + + /* + * @brief Produces the distance from being orthogonal for a given 3x3 matrix + * in row-major form. + * @param e The vector to test representing a 3x3 matrix in row major form. + * @return The distance the matrix is from being orthogonal. + */ + static double orthogonalityError(const cv::Matx& e); + + /* + * @brief Processes a solution and sorts it by error. + * @param solution The solution to evaluate. + * @param min_error The current minimum error. + */ + void checkSolution(SQPSolution& solution, double& min_error); + + /* + * @brief Computes the determinant of a matrix stored in row-major format. + * @param e Vector representing a 3x3 matrix stored in row-major format. + * @return The determinant of the matrix. + */ + static double det3x3(const cv::Matx& e); + + /* + * @brief Tests the cheirality for a given solution. + * @param solution The solution to evaluate. + */ + inline bool positiveDepth(const SQPSolution& solution) const; + + /* + * @brief Determines the nearest rotation matrix to a given rotaiton matrix. + * Input and output are 9x1 vector representing a vector stored in row-major + * form. + * @param e The input 3x3 matrix stored in a vector in row-major form. + * @param r The nearest rotation matrix to the input e (again in row-major form). + */ + static void nearestRotationMatrix(const cv::Matx& e, + cv::Matx& r); + + /* + * @brief Runs the sequential quadratic programming on orthogonal matrices. + * @param r0 The start point of the solver. + */ + SQPSolution runSQP(const cv::Matx& r0); + + /* + * @brief Steps down the gradient for the given matrix r to solve the SQP system. + * @param r The current matrix step. + * @param delta The next step down the gradient. + */ + void solveSQPSystem(const cv::Matx& r, cv::Matx& delta); + + /* + * @brief Analytically computes the inverse of a symmetric 3x3 matrix using the + * lower triangle. + * @param Q The matrix to invert. + * @param Qinv The inverse of Q. + * @param threshold The threshold to determine if Q is singular and non-invertible. + */ + bool analyticalInverse3x3Symm(const cv::Matx& Q, + cv::Matx& Qinv, + const double& threshold = 1e-8); + + /* + * @brief Computes the 3D null space and 6D normal space of the constraint Jacobian + * at a 9D vector r (representing a rank-3 matrix). Note that K is lower + * triangular so upper triangle is undefined. + * @param r 9D vector representing a rank-3 matrix. + * @param H 6D row space of the constraint Jacobian at r. + * @param N 3D null space of the constraint Jacobian at r. + * @param K The constraint Jacobian at r. + * @param norm_threshold Threshold for column vector norm of Pn (the projection onto the null space + * of the constraint Jacobian). + */ + void computeRowAndNullspace(const cv::Matx& r, + cv::Matx& H, + cv::Matx& N, + cv::Matx& K, + const double& norm_threshold = 0.1); + + static const double RANK_TOLERANCE; + static const double SQP_SQUARED_TOLERANCE; + static const double SQP_DET_THRESHOLD; + static const double ORTHOGONALITY_SQUARED_ERROR_THRESHOLD; + static const double EQUAL_VECTORS_SQUARED_DIFF; + static const double EQUAL_SQUARED_ERRORS_DIFF; + static const double POINT_VARIANCE_THRESHOLD; + static const int SQP_MAX_ITERATION; + static const double SQRT3; + + cv::Matx omega_; + cv::Vec s_; + cv::Matx u_; + cv::Matx p_; + cv::Vec3d point_mean_; + int num_null_vectors_; + + SQPSolution solutions_[18]; + int num_solutions_; + +}; + +} +} + +#endif diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 0d35fa7126..fb0e2965e6 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -190,6 +190,8 @@ static std::string printMethod(int method) return "SOLVEPNP_IPPE"; case 7: return "SOLVEPNP_IPPE_SQUARE"; + case 8: + return "SOLVEPNP_SQPNP"; default: return "Unknown value"; } @@ -206,6 +208,7 @@ public: eps[SOLVEPNP_AP3P] = 1.0e-2; eps[SOLVEPNP_DLS] = 1.0e-2; eps[SOLVEPNP_UPNP] = 1.0e-2; + eps[SOLVEPNP_SQPNP] = 1.0e-2; totalTestsCount = 10; pointsCount = 500; } @@ -436,6 +439,7 @@ public: eps[SOLVEPNP_UPNP] = 1.0e-6; //UPnP is remapped to EPnP, so we use the same threshold eps[SOLVEPNP_IPPE] = 1.0e-6; eps[SOLVEPNP_IPPE_SQUARE] = 1.0e-6; + eps[SOLVEPNP_SQPNP] = 1.0e-6; totalTestsCount = 1000;