Alexander Alekhin
4 years ago
commit
9d2eabaaa2
457 changed files with 30343 additions and 4310 deletions
@ -0,0 +1,47 @@ |
||||
/*
|
||||
* jdmerge.h |
||||
* |
||||
* This file was part of the Independent JPEG Group's software: |
||||
* Copyright (C) 1994-1996, Thomas G. Lane. |
||||
* libjpeg-turbo Modifications: |
||||
* Copyright (C) 2020, D. R. Commander. |
||||
* For conditions of distribution and use, see the accompanying README.ijg |
||||
* file. |
||||
*/ |
||||
|
||||
#define JPEG_INTERNALS |
||||
#include "jpeglib.h" |
||||
|
||||
#ifdef UPSAMPLE_MERGING_SUPPORTED |
||||
|
||||
|
||||
/* Private subobject */ |
||||
|
||||
typedef struct { |
||||
struct jpeg_upsampler pub; /* public fields */ |
||||
|
||||
/* Pointer to routine to do actual upsampling/conversion of one row group */ |
||||
void (*upmethod) (j_decompress_ptr cinfo, JSAMPIMAGE input_buf, |
||||
JDIMENSION in_row_group_ctr, JSAMPARRAY output_buf); |
||||
|
||||
/* Private state for YCC->RGB conversion */ |
||||
int *Cr_r_tab; /* => table for Cr to R conversion */ |
||||
int *Cb_b_tab; /* => table for Cb to B conversion */ |
||||
JLONG *Cr_g_tab; /* => table for Cr to G conversion */ |
||||
JLONG *Cb_g_tab; /* => table for Cb to G conversion */ |
||||
|
||||
/* For 2:1 vertical sampling, we produce two output rows at a time.
|
||||
* We need a "spare" row buffer to hold the second output row if the |
||||
* application provides just a one-row buffer; we also use the spare |
||||
* to discard the dummy last row if the image height is odd. |
||||
*/ |
||||
JSAMPROW spare_row; |
||||
boolean spare_full; /* T if spare buffer is occupied */ |
||||
|
||||
JDIMENSION out_row_width; /* samples per output row */ |
||||
JDIMENSION rows_to_go; /* counts rows remaining in image */ |
||||
} my_merged_upsampler; |
||||
|
||||
typedef my_merged_upsampler *my_merged_upsample_ptr; |
||||
|
||||
#endif /* UPSAMPLE_MERGING_SUPPORTED */ |
||||
@ -0,0 +1,36 @@ |
||||
ocv_clear_vars(HAVE_ONNX) |
||||
|
||||
set(ONNXRT_ROOT_DIR "" CACHE PATH "ONNX Runtime install directory") |
||||
|
||||
# For now, check the old name ORT_INSTALL_DIR |
||||
if(ORT_INSTALL_DIR AND NOT ONNXRT_ROOT_DIR) |
||||
set(ONNXRT_ROOT_DIR ${ORT_INSTALL_DIR}) |
||||
endif() |
||||
|
||||
if(ONNXRT_ROOT_DIR) |
||||
find_library(ORT_LIB onnxruntime |
||||
${ONNXRT_ROOT_DIR}/lib |
||||
CMAKE_FIND_ROOT_PATH_BOTH) |
||||
find_path(ORT_INCLUDE onnxruntime_cxx_api.h |
||||
${ONNXRT_ROOT_DIR}/include/onnxruntime/core/session |
||||
CMAKE_FIND_ROOT_PATH_BOTH) |
||||
endif() |
||||
|
||||
if(ORT_LIB AND ORT_INCLUDE) |
||||
set(HAVE_ONNX TRUE) |
||||
# For CMake output only |
||||
set(ONNX_LIBRARIES "${ORT_LIB}" CACHE STRING "ONNX Runtime libraries") |
||||
set(ONNX_INCLUDE_DIR "${ORT_INCLUDE}" CACHE STRING "ONNX Runtime include path") |
||||
|
||||
# Link target with associated interface headers |
||||
set(ONNX_LIBRARY "onnxruntime" CACHE STRING "ONNX Link Target") |
||||
ocv_add_library(${ONNX_LIBRARY} SHARED IMPORTED) |
||||
set_target_properties(${ONNX_LIBRARY} PROPERTIES |
||||
INTERFACE_INCLUDE_DIRECTORIES ${ORT_INCLUDE} |
||||
IMPORTED_LOCATION ${ORT_LIB} |
||||
IMPORTED_IMPLIB ${ORT_LIB}) |
||||
endif() |
||||
|
||||
if(NOT HAVE_ONNX) |
||||
ocv_clear_vars(HAVE_ONNX ORT_LIB ORT_INCLUDE_DIR) |
||||
endif() |
||||
|
Before Width: | Height: | Size: 15 KiB After Width: | Height: | Size: 18 KiB |
|
After Width: | Height: | Size: 135 KiB |
|
After Width: | Height: | Size: 29 KiB |
@ -0,0 +1,150 @@ |
||||
Using Orbbec Astra 3D cameras {#tutorial_orbbec_astra} |
||||
====================================================== |
||||
|
||||
@prev_tutorial{tutorial_kinect_openni} |
||||
@next_tutorial{tutorial_intelperc} |
||||
|
||||
|
||||
### Introduction |
||||
|
||||
This tutorial is devoted to the Astra Series of Orbbec 3D cameras (https://orbbec3d.com/product-astra-pro/). |
||||
That cameras have a depth sensor in addition to a common color sensor. The depth sensors can be read using |
||||
the OpenNI interface with @ref cv::VideoCapture class. The video stream is provided through the regular camera |
||||
interface. |
||||
|
||||
### Installation Instructions |
||||
|
||||
In order to use a depth sensor with OpenCV you should do the following steps: |
||||
|
||||
-# Download the latest version of Orbbec OpenNI SDK (from here <https://orbbec3d.com/develop/>). |
||||
Unzip the archive, choose the build according to your operating system and follow installation |
||||
steps provided in the Readme file. For instance, if you use 64bit GNU/Linux run: |
||||
@code{.bash} |
||||
$ cd Linux/OpenNI-Linux-x64-2.3.0.63/ |
||||
$ sudo ./install.sh |
||||
@endcode |
||||
When you are done with the installation, make sure to replug your device for udev rules to take |
||||
effect. The camera should now work as a general camera device. Note that your current user should |
||||
belong to group `video` to have access to the camera. Also, make sure to source `OpenNIDevEnvironment` file: |
||||
@code{.bash} |
||||
$ source OpenNIDevEnvironment |
||||
@endcode |
||||
|
||||
-# Run the following commands to verify that OpenNI library and header files can be found. You should see |
||||
something similar in your terminal: |
||||
@code{.bash} |
||||
$ echo $OPENNI2_INCLUDE |
||||
/home/user/OpenNI_2.3.0.63/Linux/OpenNI-Linux-x64-2.3.0.63/Include |
||||
$ echo $OPENNI2_REDIST |
||||
/home/user/OpenNI_2.3.0.63/Linux/OpenNI-Linux-x64-2.3.0.63/Redist |
||||
@endcode |
||||
If the above two variables are empty, then you need to source `OpenNIDevEnvironment` again. Now you can |
||||
configure OpenCV with OpenNI support enabled by setting the `WITH_OPENNI2` flag in CMake. |
||||
You may also like to enable the `BUILD_EXAMPLES` flag to get a code sample working with your Astra camera. |
||||
Run the following commands in the directory containing OpenCV source code to enable OpenNI support: |
||||
@code{.bash} |
||||
$ mkdir build |
||||
$ cd build |
||||
$ cmake -DWITH_OPENNI2=ON .. |
||||
@endcode |
||||
If the OpenNI library is found, OpenCV will be built with OpenNI2 support. You can see the status of OpenNI2 |
||||
support in the CMake log: |
||||
@code{.text} |
||||
-- Video I/O: |
||||
-- DC1394: YES (2.2.6) |
||||
-- FFMPEG: YES |
||||
-- avcodec: YES (58.91.100) |
||||
-- avformat: YES (58.45.100) |
||||
-- avutil: YES (56.51.100) |
||||
-- swscale: YES (5.7.100) |
||||
-- avresample: NO |
||||
-- GStreamer: YES (1.18.1) |
||||
-- OpenNI2: YES (2.3.0) |
||||
-- v4l/v4l2: YES (linux/videodev2.h) |
||||
@endcode |
||||
|
||||
-# Build OpenCV: |
||||
@code{.bash} |
||||
$ make |
||||
@endcode |
||||
|
||||
### Code |
||||
|
||||
To get both depth and color frames, two @ref cv::VideoCapture objects should be created: |
||||
|
||||
@snippetlineno samples/cpp/tutorial_code/videoio/orbbec_astra/orbbec_astra.cpp Open streams |
||||
|
||||
The first object will use the regular Video4Linux2 interface to access the color sensor. The second one |
||||
is using OpenNI2 API to retrieve depth data. |
||||
|
||||
Before using the created VideoCapture objects you may want to setup stream parameters by setting |
||||
objects' properties. The most important parameters are frame width, frame height and fps: |
||||
|
||||
@snippetlineno samples/cpp/tutorial_code/videoio/orbbec_astra/orbbec_astra.cpp Setup streams |
||||
|
||||
For setting and getting some property of sensor data generators use @ref cv::VideoCapture::set and |
||||
@ref cv::VideoCapture::get methods respectively, e.g. : |
||||
|
||||
@snippetlineno samples/cpp/tutorial_code/videoio/orbbec_astra/orbbec_astra.cpp Get properties |
||||
|
||||
The following properties of cameras available through OpenNI interfaces are supported for the depth |
||||
generator: |
||||
|
||||
- @ref cv::CAP_PROP_FRAME_WIDTH -- Frame width in pixels. |
||||
- @ref cv::CAP_PROP_FRAME_HEIGHT -- Frame height in pixels. |
||||
- @ref cv::CAP_PROP_FPS -- Frame rate in FPS. |
||||
- @ref cv::CAP_PROP_OPENNI_REGISTRATION -- Flag that registers the remapping depth map to image map |
||||
by changing the depth generator's viewpoint (if the flag is "on") or sets this view point to |
||||
its normal one (if the flag is "off"). The registration process’ resulting images are |
||||
pixel-aligned, which means that every pixel in the image is aligned to a pixel in the depth |
||||
image. |
||||
- @ref cv::CAP_PROP_OPENNI2_MIRROR -- Flag to enable or disable mirroring for this stream. Set to 0 |
||||
to disable mirroring |
||||
|
||||
Next properties are available for getting only: |
||||
|
||||
- @ref cv::CAP_PROP_OPENNI_FRAME_MAX_DEPTH -- A maximum supported depth of the camera in mm. |
||||
- @ref cv::CAP_PROP_OPENNI_BASELINE -- Baseline value in mm. |
||||
|
||||
After the VideoCapture objects are set up you can start reading frames from them. |
||||
|
||||
@note |
||||
OpenCV's VideoCapture provides synchronous API, so you have to grab frames in a new thread |
||||
to avoid one stream blocking while another stream is being read. VideoCapture is not a |
||||
thread-safe class, so you need to be careful to avoid any possible deadlocks or data races. |
||||
|
||||
Example implementation that gets frames from each sensor in a new thread and stores them |
||||
in a list along with their timestamps: |
||||
|
||||
@snippetlineno samples/cpp/tutorial_code/videoio/orbbec_astra/orbbec_astra.cpp Read streams |
||||
|
||||
VideoCapture can retrieve the following data: |
||||
|
||||
-# data given from the depth generator: |
||||
- @ref cv::CAP_OPENNI_DEPTH_MAP - depth values in mm (CV_16UC1) |
||||
- @ref cv::CAP_OPENNI_POINT_CLOUD_MAP - XYZ in meters (CV_32FC3) |
||||
- @ref cv::CAP_OPENNI_DISPARITY_MAP - disparity in pixels (CV_8UC1) |
||||
- @ref cv::CAP_OPENNI_DISPARITY_MAP_32F - disparity in pixels (CV_32FC1) |
||||
- @ref cv::CAP_OPENNI_VALID_DEPTH_MASK - mask of valid pixels (not occluded, not shaded, etc.) |
||||
(CV_8UC1) |
||||
|
||||
-# data given from the color sensor is a regular BGR image (CV_8UC3). |
||||
|
||||
When new data is available a reading thread notifies the main thread. A frame is stored in the |
||||
ordered list -- the first frame is the latest one: |
||||
|
||||
@snippetlineno samples/cpp/tutorial_code/videoio/orbbec_astra/orbbec_astra.cpp Show color frame |
||||
|
||||
Depth frames can be picked the same way from the `depthFrames` list. |
||||
|
||||
After that, you'll have two frames: one containing color information and another one -- depth |
||||
information. In the sample images below you can see the color frame and the depth frame showing |
||||
the same scene. Looking at the color frame it's hard to distinguish plant leaves from leaves painted |
||||
on a wall, but the depth data makes it easy. |
||||
|
||||
 |
||||
 |
||||
|
||||
The complete implementation can be found in |
||||
[orbbec_astra.cpp](https://github.com/opencv/opencv/tree/master/samples/cpp/tutorial_code/videoio/orbbec_astra/orbbec_astra.cpp) |
||||
in `samples/cpp/tutorial_code/videoio` directory. |
||||
@ -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 <opencv2/calib3d.hpp> |
||||
|
||||
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 <typename tp, int snrows, int sncols, |
||||
int dnrows, int dncols> |
||||
void set(int row, int col, cv::Matx<tp, dnrows, dncols>& dest, |
||||
const cv::Matx<tp, snrows, sncols>& 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<double, 9, 9>::zeros(); |
||||
cv::Matx<double, 3, 9> qa_sum = cv::Matx<double, 3, 9>::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<cv::Point2d>(i); |
||||
const cv::Point3d& obj_pt = _objectPoints.at<cv::Point3d>(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<double, 3, 3> 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<double, 3, 3> 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<double>::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<double, 9, 1> 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<double, 9, 1> 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<double, 9, 1> e = u_.col(index); |
||||
SQPSolution solutions[2]; |
||||
|
||||
Matx<double, 9, 1> 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<double, 9, 1>& r0) |
||||
{ |
||||
cv::Matx<double, 9, 1> r = r0; |
||||
|
||||
double delta_squared_norm = std::numeric_limits<double>::max(); |
||||
cv::Matx<double, 9, 1> 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<double, 9, 1>& r, cv::Matx<double, 9, 1>& 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<double, 9, 3> N; |
||||
cv::Matx<double, 9, 6> H; |
||||
cv::Matx<double, 6, 6> JH; |
||||
|
||||
computeRowAndNullspace(r, H, N, JH); |
||||
|
||||
cv::Matx<double, 6, 1> 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<double, 6, 1> 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<double, 3, 9> nt_omega = N.t() * omega_; |
||||
cv::Matx<double, 3, 3> W = nt_omega * N, W_inv; |
||||
|
||||
analyticalInverse3x3Symm(W, W_inv); |
||||
|
||||
cv::Matx<double, 3, 1> y = -W_inv * nt_omega * (delta + r); |
||||
delta += N * y; |
||||
} |
||||
|
||||
bool PoseSolver::analyticalInverse3x3Symm(const cv::Matx<double, 3, 3>& Q, |
||||
cv::Matx<double, 3, 3>& 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<double, 9, 1>& r, |
||||
cv::Matx<double, 9, 6>& H, |
||||
cv::Matx<double, 9, 3>& N, |
||||
cv::Matx<double, 6, 6>& K, |
||||
const double& norm_threshold) |
||||
{ |
||||
H = cv::Matx<double, 9, 6>::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<double, 9, 1> q4 = H.col(4); |
||||
q4 /= cv::norm(q4); |
||||
set<double, 9, 1, 9, 6>(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<double, 9, 1> q5 = H.col(5); |
||||
q5 /= cv::norm(q5); |
||||
set<double, 9, 1, 9, 6>(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<double, 9, 9> Pn = cv::Matx<double, 9, 9>::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<double>::min(); |
||||
double min_dot12 = std::numeric_limits<double>::max(); |
||||
double min_dot1323 = std::numeric_limits<double>::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<double, 9, 1> v1 = Pn.col(index1); |
||||
v1 /= max_norm1; |
||||
set<double, 9, 1, 9, 3>(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<double, 9, 1> v2 = Pn.col(index2); |
||||
Matx<double, 9, 1> n0 = N.col(0); |
||||
v2 -= v2.dot(n0) * n0; |
||||
v2 /= cv::norm(v2); |
||||
set<double, 9, 1, 9, 3>(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<double, 9, 1> v3 = Pn.col(index3); |
||||
Matx<double, 9, 1> n1 = N.col(1); |
||||
v3 -= (v3.dot(n1)) * n1 - (v3.dot(n0)) * n0; |
||||
v3 /= cv::norm(v3); |
||||
set<double, 9, 1, 9, 3>(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<double, 9, 1>& e, |
||||
cv::Matx<double, 9, 1>& r) |
||||
{ |
||||
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<double, 9, 1>& 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<double, 9, 1>& r = solution.r_hat; |
||||
const cv::Matx<double, 3, 1>& 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<double, 9, 1>& 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); |
||||
} |
||||
|
||||
} |
||||
} |
||||
@ -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 <opencv2/core.hpp> |
||||
|
||||
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<double, 9, 1> r_hat; |
||||
cv::Matx<double, 3, 1> 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<double, 9, 1>& 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<double, 9, 1>& 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<double, 9, 1>& e, |
||||
cv::Matx<double, 9, 1>& r); |
||||
|
||||
/*
|
||||
* @brief Runs the sequential quadratic programming on orthogonal matrices. |
||||
* @param r0 The start point of the solver. |
||||
*/ |
||||
SQPSolution runSQP(const cv::Matx<double, 9, 1>& 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<double, 9, 1>& r, cv::Matx<double, 9, 1>& 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<double, 3, 3>& Q, |
||||
cv::Matx<double, 3, 3>& 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<double, 9, 1>& r, |
||||
cv::Matx<double, 9, 6>& H, |
||||
cv::Matx<double, 9, 3>& N, |
||||
cv::Matx<double, 6, 6>& 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<double, 9, 9> omega_; |
||||
cv::Vec<double, 9> s_; |
||||
cv::Matx<double, 9, 9> u_; |
||||
cv::Matx<double, 3, 9> p_; |
||||
cv::Vec3d point_mean_; |
||||
int num_null_vectors_; |
||||
|
||||
SQPSolution solutions_[18]; |
||||
int num_solutions_; |
||||
|
||||
}; |
||||
|
||||
} |
||||
} |
||||
|
||||
#endif |
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,849 @@ |
||||
// 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.
|
||||
//
|
||||
//
|
||||
// License Agreement
|
||||
// For Open Source Computer Vision Library
|
||||
//
|
||||
// Copyright (C) 2020, Huawei Technologies Co., Ltd. All rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Author: Liangqian Kong <chargerKong@126.com>
|
||||
// Longbu Wang <riskiest@gmail.com>
|
||||
|
||||
#ifndef OPENCV_CORE_QUATERNION_INL_HPP |
||||
#define OPENCV_CORE_QUATERNION_INL_HPP |
||||
|
||||
#ifndef OPENCV_CORE_QUATERNION_HPP |
||||
#erorr This is not a standalone header. Include quaternion.hpp instead. |
||||
#endif |
||||
|
||||
//@cond IGNORE
|
||||
///////////////////////////////////////////////////////////////////////////////////////
|
||||
//Implementation
|
||||
namespace cv { |
||||
|
||||
template <typename T> |
||||
Quat<T>::Quat() : w(0), x(0), y(0), z(0) {} |
||||
|
||||
template <typename T> |
||||
Quat<T>::Quat(const Vec<T, 4> &coeff):w(coeff[0]), x(coeff[1]), y(coeff[2]), z(coeff[3]){} |
||||
|
||||
template <typename T> |
||||
Quat<T>::Quat(const T qw, const T qx, const T qy, const T qz):w(qw), x(qx), y(qy), z(qz){} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::createFromAngleAxis(const T angle, const Vec<T, 3> &axis) |
||||
{ |
||||
T w, x, y, z; |
||||
T vNorm = std::sqrt(axis.dot(axis)); |
||||
if (vNorm < CV_QUAT_EPS) |
||||
{ |
||||
CV_Error(Error::StsBadArg, "this quaternion does not represent a rotation"); |
||||
} |
||||
const T angle_half = angle * 0.5; |
||||
w = std::cos(angle_half); |
||||
const T sin_v = std::sin(angle_half); |
||||
const T sin_norm = sin_v / vNorm; |
||||
x = sin_norm * axis[0]; |
||||
y = sin_norm * axis[1]; |
||||
z = sin_norm * axis[2]; |
||||
return Quat<T>(w, x, y, z); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::createFromRotMat(InputArray _R) |
||||
{ |
||||
CV_CheckTypeEQ(_R.type(), cv::traits::Type<T>::value, ""); |
||||
if (_R.rows() != 3 || _R.cols() != 3) |
||||
{ |
||||
CV_Error(Error::StsBadArg, "Cannot convert matrix to quaternion: rotation matrix should be a 3x3 matrix"); |
||||
} |
||||
Matx<T, 3, 3> R; |
||||
_R.copyTo(R); |
||||
|
||||
T S, w, x, y, z; |
||||
T trace = R(0, 0) + R(1, 1) + R(2, 2); |
||||
if (trace > 0) |
||||
{ |
||||
S = std::sqrt(trace + 1) * 2; |
||||
x = (R(1, 2) - R(2, 1)) / S; |
||||
y = (R(2, 0) - R(0, 2)) / S; |
||||
z = (R(0, 1) - R(1, 0)) / S; |
||||
w = -0.25 * S; |
||||
} |
||||
else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) |
||||
{ |
||||
|
||||
S = std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2; |
||||
x = -0.25 * S; |
||||
y = -(R(1, 0) + R(0, 1)) / S; |
||||
z = -(R(0, 2) + R(2, 0)) / S; |
||||
w = (R(1, 2) - R(2, 1)) / S; |
||||
} |
||||
else if (R(1, 1) > R(2, 2)) |
||||
{ |
||||
S = std::sqrt(1.0 - R(0, 0) + R(1, 1) - R(2, 2)) * 2; |
||||
x = (R(0, 1) + R(1, 0)) / S; |
||||
y = 0.25 * S; |
||||
z = (R(1, 2) + R(2, 1)) / S; |
||||
w = (R(0, 2) - R(2, 0)) / S; |
||||
} |
||||
else |
||||
{ |
||||
S = std::sqrt(1.0 - R(0, 0) - R(1, 1) + R(2, 2)) * 2; |
||||
x = (R(0, 2) + R(2, 0)) / S; |
||||
y = (R(1, 2) + R(2, 1)) / S; |
||||
z = 0.25 * S; |
||||
w = -(R(0, 1) - R(1, 0)) / S; |
||||
} |
||||
return Quat<T> (w, x, y, z); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::createFromRvec(InputArray _rvec) |
||||
{ |
||||
if (!((_rvec.cols() == 1 && _rvec.rows() == 3) || (_rvec.cols() == 3 && _rvec.rows() == 1))) { |
||||
CV_Error(Error::StsBadArg, "Cannot convert rotation vector to quaternion: The length of rotation vector should be 3"); |
||||
} |
||||
Vec<T, 3> rvec; |
||||
_rvec.copyTo(rvec); |
||||
T psi = std::sqrt(rvec.dot(rvec)); |
||||
if (abs(psi) < CV_QUAT_EPS) { |
||||
return Quat<T> (1, 0, 0, 0); |
||||
} |
||||
Vec<T, 3> axis = rvec / psi; |
||||
return createFromAngleAxis(psi, axis); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::operator-() const |
||||
{ |
||||
return Quat<T>(-w, -x, -y, -z); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline bool Quat<T>::operator==(const Quat<T> &q) const |
||||
{ |
||||
return (abs(w - q.w) < CV_QUAT_EPS && abs(x - q.x) < CV_QUAT_EPS && abs(y - q.y) < CV_QUAT_EPS && abs(z - q.z) < CV_QUAT_EPS); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::operator+(const Quat<T> &q1) const |
||||
{ |
||||
return Quat<T>(w + q1.w, x + q1.x, y + q1.y, z + q1.z); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::operator-(const Quat<T> &q1) const |
||||
{ |
||||
return Quat<T>(w - q1.w, x - q1.x, y - q1.y, z - q1.z); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T>& Quat<T>::operator+=(const Quat<T> &q1) |
||||
{ |
||||
w += q1.w; |
||||
x += q1.x; |
||||
y += q1.y; |
||||
z += q1.z; |
||||
return *this; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T>& Quat<T>::operator-=(const Quat<T> &q1) |
||||
{ |
||||
w -= q1.w; |
||||
x -= q1.x; |
||||
y -= q1.y; |
||||
z -= q1.z; |
||||
return *this; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::operator*(const Quat<T> &q1) const |
||||
{ |
||||
Vec<T, 4> q{w, x, y, z}; |
||||
Vec<T, 4> q2{q1.w, q1.x, q1.y, q1.z}; |
||||
return Quat<T>(q * q2); |
||||
} |
||||
|
||||
|
||||
template <typename T, typename S> |
||||
Quat<T> operator*(const Quat<T> &q1, const S a) |
||||
{ |
||||
return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z); |
||||
} |
||||
|
||||
template <typename T, typename S> |
||||
Quat<T> operator*(const S a, const Quat<T> &q1) |
||||
{ |
||||
return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T>& Quat<T>::operator*=(const Quat<T> &q1) |
||||
{ |
||||
T qw, qx, qy, qz; |
||||
qw = w * q1.w - x * q1.x - y * q1.y - z * q1.z; |
||||
qx = x * q1.w + w * q1.x + y * q1.z - z * q1.y; |
||||
qy = y * q1.w + w * q1.y + z * q1.x - x * q1.z; |
||||
qz = z * q1.w + w * q1.z + x * q1.y - y * q1.x; |
||||
w = qw; |
||||
x = qx; |
||||
y = qy; |
||||
z = qz; |
||||
return *this; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T>& Quat<T>::operator/=(const Quat<T> &q1) |
||||
{ |
||||
Quat<T> q(*this * q1.inv()); |
||||
w = q.w; |
||||
x = q.x; |
||||
y = q.y; |
||||
z = q.z; |
||||
return *this; |
||||
} |
||||
template <typename T> |
||||
Quat<T>& Quat<T>::operator*=(const T &q1) |
||||
{ |
||||
w *= q1; |
||||
x *= q1; |
||||
y *= q1; |
||||
z *= q1; |
||||
return *this; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T>& Quat<T>::operator/=(const T &a) |
||||
{ |
||||
const T a_inv = 1.0 / a; |
||||
w *= a_inv; |
||||
x *= a_inv; |
||||
y *= a_inv; |
||||
z *= a_inv; |
||||
return *this; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::operator/(const T &a) const |
||||
{ |
||||
const T a_inv = 1.0 / a; |
||||
return Quat<T>(w * a_inv, x * a_inv, y * a_inv, z * a_inv); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::operator/(const Quat<T> &q) const |
||||
{ |
||||
return *this * q.inv(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline const T& Quat<T>::operator[](std::size_t n) const |
||||
{ |
||||
switch (n) { |
||||
case 0: |
||||
return w; |
||||
case 1: |
||||
return x; |
||||
case 2: |
||||
return y; |
||||
case 3: |
||||
return z; |
||||
default: |
||||
CV_Error(Error::StsOutOfRange, "subscript exceeds the index range"); |
||||
} |
||||
} |
||||
|
||||
template <typename T> |
||||
inline T& Quat<T>::operator[](std::size_t n) |
||||
{ |
||||
switch (n) { |
||||
case 0: |
||||
return w; |
||||
case 1: |
||||
return x; |
||||
case 2: |
||||
return y; |
||||
case 3: |
||||
return z; |
||||
default: |
||||
CV_Error(Error::StsOutOfRange, "subscript exceeds the index range"); |
||||
} |
||||
} |
||||
|
||||
template <typename T> |
||||
std::ostream & operator<<(std::ostream &os, const Quat<T> &q) |
||||
{ |
||||
os << "Quat " << Vec<T, 4>{q.w, q.x, q.y, q.z}; |
||||
return os; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline T Quat<T>::at(size_t index) const |
||||
{ |
||||
return (*this)[index]; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::conjugate() const |
||||
{ |
||||
return Quat<T>(w, -x, -y, -z); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline T Quat<T>::norm() const |
||||
{ |
||||
return std::sqrt(dot(*this)); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> exp(const Quat<T> &q) |
||||
{ |
||||
return q.exp(); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::exp() const |
||||
{ |
||||
Vec<T, 3> v{x, y, z}; |
||||
T normV = std::sqrt(v.dot(v)); |
||||
T k = normV < CV_QUAT_EPS ? 1 : std::sin(normV) / normV; |
||||
return std::exp(w) * Quat<T>(std::cos(normV), v[0] * k, v[1] * k, v[2] * k); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> log(const Quat<T> &q, QuatAssumeType assumeUnit) |
||||
{ |
||||
return q.log(assumeUnit); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::log(QuatAssumeType assumeUnit) const |
||||
{ |
||||
Vec<T, 3> v{x, y, z}; |
||||
T vNorm = std::sqrt(v.dot(v)); |
||||
if (assumeUnit) |
||||
{ |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : std::acos(w) / vNorm; |
||||
return Quat<T>(0, v[0] * k, v[1] * k, v[2] * k); |
||||
} |
||||
T qNorm = norm(); |
||||
if (qNorm < CV_QUAT_EPS) |
||||
{ |
||||
CV_Error(Error::StsBadArg, "Cannot apply this quaternion to log function: undefined"); |
||||
} |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : std::acos(w / qNorm) / vNorm; |
||||
return Quat<T>(std::log(qNorm), v[0] * k, v[1] * k, v[2] *k); |
||||
} |
||||
|
||||
template <typename T, typename _T> |
||||
inline Quat<T> power(const Quat<T> &q1, _T alpha, QuatAssumeType assumeUnit) |
||||
{ |
||||
return q1.power(alpha, assumeUnit); |
||||
} |
||||
|
||||
template <typename T> |
||||
template <typename _T> |
||||
inline Quat<T> Quat<T>::power(_T alpha, QuatAssumeType assumeUnit) const |
||||
{ |
||||
if (x * x + y * y + z * z > CV_QUAT_EPS) |
||||
{ |
||||
T angle = getAngle(assumeUnit); |
||||
Vec<T, 3> axis = getAxis(assumeUnit); |
||||
if (assumeUnit) |
||||
{ |
||||
return createFromAngleAxis(alpha * angle, axis); |
||||
} |
||||
return std::pow(norm(), alpha) * createFromAngleAxis(alpha * angle, axis); |
||||
} |
||||
else |
||||
{ |
||||
return std::pow(norm(), alpha) * Quat<T>(w, x, y, z); |
||||
} |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> sqrt(const Quat<T> &q, QuatAssumeType assumeUnit) |
||||
{ |
||||
return q.sqrt(assumeUnit); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::sqrt(QuatAssumeType assumeUnit) const |
||||
{ |
||||
return power(0.5, assumeUnit); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> power(const Quat<T> &p, const Quat<T> &q, QuatAssumeType assumeUnit) |
||||
{ |
||||
return p.power(q, assumeUnit); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::power(const Quat<T> &q, QuatAssumeType assumeUnit) const |
||||
{ |
||||
return cv::exp(q * log(assumeUnit)); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline T Quat<T>::dot(Quat<T> q1) const |
||||
{ |
||||
return w * q1.w + x * q1.x + y * q1.y + z * q1.z; |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> crossProduct(const Quat<T> &p, const Quat<T> &q) |
||||
{ |
||||
return p.crossProduct(q); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::crossProduct(const Quat<T> &q) const |
||||
{ |
||||
return Quat<T> (0, y * q.z - z * q.y, z * q.x - x * q.z, x * q.y - q.x * y); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::normalize() const |
||||
{ |
||||
T normVal = norm(); |
||||
if (normVal < CV_QUAT_EPS) |
||||
{ |
||||
CV_Error(Error::StsBadArg, "Cannot normalize this quaternion: the norm is too small."); |
||||
} |
||||
return Quat<T>(w / normVal, x / normVal, y / normVal, z / normVal) ; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> inv(const Quat<T> &q, QuatAssumeType assumeUnit) |
||||
{ |
||||
return q.inv(assumeUnit); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::inv(QuatAssumeType assumeUnit) const |
||||
{ |
||||
if (assumeUnit) |
||||
{ |
||||
return conjugate(); |
||||
} |
||||
T norm2 = dot(*this); |
||||
if (norm2 < CV_QUAT_EPS) |
||||
{ |
||||
CV_Error(Error::StsBadArg, "This quaternion do not have inverse quaternion"); |
||||
} |
||||
return conjugate() / norm2; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> sinh(const Quat<T> &q) |
||||
{ |
||||
return q.sinh(); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::sinh() const |
||||
{ |
||||
Vec<T, 3> v{x, y ,z}; |
||||
T vNorm = std::sqrt(v.dot(v)); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : std::cosh(w) * std::sin(vNorm) / vNorm; |
||||
return Quat<T>(std::sinh(w) * std::cos(vNorm), v[0] * k, v[1] * k, v[2] * k); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> cosh(const Quat<T> &q) |
||||
{ |
||||
return q.cosh(); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::cosh() const |
||||
{ |
||||
Vec<T, 3> v{x, y ,z}; |
||||
T vNorm = std::sqrt(v.dot(v)); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : std::sinh(w) * std::sin(vNorm) / vNorm; |
||||
return Quat<T>(std::cosh(w) * std::cos(vNorm), v[0] * k, v[1] * k, v[2] * k); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> tanh(const Quat<T> &q) |
||||
{ |
||||
return q.tanh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::tanh() const |
||||
{ |
||||
return sinh() * cosh().inv(); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> sin(const Quat<T> &q) |
||||
{ |
||||
return q.sin(); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::sin() const |
||||
{ |
||||
Vec<T, 3> v{x, y ,z}; |
||||
T vNorm = std::sqrt(v.dot(v)); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : std::cos(w) * std::sinh(vNorm) / vNorm; |
||||
return Quat<T>(std::sin(w) * std::cosh(vNorm), v[0] * k, v[1] * k, v[2] * k); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> cos(const Quat<T> &q) |
||||
{ |
||||
return q.cos(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::cos() const |
||||
{ |
||||
Vec<T, 3> v{x, y ,z}; |
||||
T vNorm = std::sqrt(v.dot(v)); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : std::sin(w) * std::sinh(vNorm) / vNorm; |
||||
return Quat<T>(std::cos(w) * std::cosh(vNorm), -v[0] * k, -v[1] * k, -v[2] * k); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> tan(const Quat<T> &q) |
||||
{ |
||||
return q.tan(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::tan() const |
||||
{ |
||||
return sin() * cos().inv(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> asinh(const Quat<T> &q) |
||||
{ |
||||
return q.asinh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::asinh() const |
||||
{ |
||||
return cv::log(*this + cv::power(*this * *this + Quat<T>(1, 0, 0, 0), 0.5)); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> acosh(const Quat<T> &q) |
||||
{ |
||||
return q.acosh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::acosh() const |
||||
{ |
||||
return cv::log(*this + cv::power(*this * *this - Quat<T>(1,0,0,0), 0.5)); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> atanh(const Quat<T> &q) |
||||
{ |
||||
return q.atanh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::atanh() const |
||||
{ |
||||
Quat<T> ident(1, 0, 0, 0); |
||||
Quat<T> c1 = (ident + *this).log(); |
||||
Quat<T> c2 = (ident - *this).log(); |
||||
return 0.5 * (c1 - c2); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> asin(const Quat<T> &q) |
||||
{ |
||||
return q.asin(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::asin() const |
||||
{ |
||||
Quat<T> v(0, x, y, z); |
||||
T vNorm = v.norm(); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : vNorm; |
||||
return -v / k * (*this * v / k).asinh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> acos(const Quat<T> &q) |
||||
{ |
||||
return q.acos(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::acos() const |
||||
{ |
||||
Quat<T> v(0, x, y, z); |
||||
T vNorm = v.norm(); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : vNorm; |
||||
return -v / k * acosh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> atan(const Quat<T> &q) |
||||
{ |
||||
return q.atan(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::atan() const |
||||
{ |
||||
Quat<T> v(0, x, y, z); |
||||
T vNorm = v.norm(); |
||||
T k = vNorm < CV_QUAT_EPS ? 1 : vNorm; |
||||
return -v / k * (*this * v / k).atanh(); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline T Quat<T>::getAngle(QuatAssumeType assumeUnit) const |
||||
{ |
||||
if (assumeUnit) |
||||
{ |
||||
return 2 * std::acos(w); |
||||
} |
||||
if (norm() < CV_QUAT_EPS) |
||||
{ |
||||
CV_Error(Error::StsBadArg, "This quaternion does not represent a rotation"); |
||||
} |
||||
return 2 * std::acos(w / norm()); |
||||
} |
||||
|
||||
template <typename T> |
||||
inline Vec<T, 3> Quat<T>::getAxis(QuatAssumeType assumeUnit) const |
||||
{ |
||||
T angle = getAngle(assumeUnit); |
||||
const T sin_v = std::sin(angle * 0.5); |
||||
if (assumeUnit) |
||||
{ |
||||
return Vec<T, 3>{x, y, z} / sin_v; |
||||
} |
||||
return Vec<T, 3> {x, y, z} / (norm() * sin_v); |
||||
} |
||||
|
||||
template <typename T> |
||||
Matx<T, 4, 4> Quat<T>::toRotMat4x4(QuatAssumeType assumeUnit) const |
||||
{ |
||||
T a = w, b = x, c = y, d = z; |
||||
if (!assumeUnit) |
||||
{ |
||||
Quat<T> qTemp = normalize(); |
||||
a = qTemp.w; |
||||
b = qTemp.x; |
||||
c = qTemp.y; |
||||
d = qTemp.z; |
||||
} |
||||
Matx<T, 4, 4> R{ |
||||
1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c) , 0, |
||||
2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b) , 0, |
||||
2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c), 0, |
||||
0 , 0 , 0 , 1, |
||||
}; |
||||
return R; |
||||
} |
||||
|
||||
template <typename T> |
||||
Matx<T, 3, 3> Quat<T>::toRotMat3x3(QuatAssumeType assumeUnit) const |
||||
{ |
||||
T a = w, b = x, c = y, d = z; |
||||
if (!assumeUnit) |
||||
{ |
||||
Quat<T> qTemp = normalize(); |
||||
a = qTemp.w; |
||||
b = qTemp.x; |
||||
c = qTemp.y; |
||||
d = qTemp.z; |
||||
} |
||||
Matx<T, 3, 3> R{ |
||||
1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c), |
||||
2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b), |
||||
2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c) |
||||
}; |
||||
return R; |
||||
} |
||||
|
||||
template <typename T> |
||||
Vec<T, 3> Quat<T>::toRotVec(QuatAssumeType assumeUnit) const |
||||
{ |
||||
T angle = getAngle(assumeUnit); |
||||
Vec<T, 3> axis = getAxis(assumeUnit); |
||||
return angle * axis; |
||||
} |
||||
|
||||
template <typename T> |
||||
Vec<T, 4> Quat<T>::toVec() const |
||||
{ |
||||
return Vec<T, 4>{w, x, y, z}; |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::lerp(const Quat<T> &q0, const Quat<T> &q1, const T t) |
||||
{ |
||||
return (1 - t) * q0 + t * q1; |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::slerp(const Quat<T> &q0, const Quat<T> &q1, const T t, QuatAssumeType assumeUnit, bool directChange) |
||||
{ |
||||
Quatd v0(q0); |
||||
Quatd v1(q1); |
||||
if (!assumeUnit) |
||||
{ |
||||
v0 = v0.normalize(); |
||||
v1 = v1.normalize(); |
||||
} |
||||
T cosTheta = v0.dot(v1); |
||||
constexpr T DOT_THRESHOLD = 0.995; |
||||
if (cosTheta > DOT_THRESHOLD) |
||||
{ |
||||
return nlerp(v0, v1, t, QUAT_ASSUME_UNIT); |
||||
} |
||||
|
||||
if (directChange && cosTheta < 0) |
||||
{ |
||||
v0 = -v0; |
||||
cosTheta = -cosTheta; |
||||
} |
||||
T sinTheta = std::sqrt(1 - cosTheta * cosTheta); |
||||
T angle = atan2(sinTheta, cosTheta); |
||||
return (std::sin((1 - t) * angle) / (sinTheta) * v0 + std::sin(t * angle) / (sinTheta) * v1).normalize(); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::nlerp(const Quat<T> &q0, const Quat<T> &q1, const T t, QuatAssumeType assumeUnit) |
||||
{ |
||||
Quat<T> v0(q0), v1(q1); |
||||
if (v1.dot(v0) < 0) |
||||
{ |
||||
v0 = -v0; |
||||
} |
||||
if (assumeUnit) |
||||
{ |
||||
return ((1 - t) * v0 + t * v1).normalize(); |
||||
} |
||||
v0 = v0.normalize(); |
||||
v1 = v1.normalize(); |
||||
return ((1 - t) * v0 + t * v1).normalize(); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline bool Quat<T>::isNormal(T eps) const |
||||
{ |
||||
|
||||
double normVar = norm(); |
||||
if ((normVar > 1 - eps) && (normVar < 1 + eps)) |
||||
return true; |
||||
return false; |
||||
} |
||||
|
||||
template <typename T> |
||||
inline void Quat<T>::assertNormal(T eps) const |
||||
{ |
||||
if (!isNormal(eps)) |
||||
CV_Error(Error::StsBadArg, "Quaternion should be normalized"); |
||||
} |
||||
|
||||
|
||||
template <typename T> |
||||
inline Quat<T> Quat<T>::squad(const Quat<T> &q0, const Quat<T> &q1, |
||||
const Quat<T> &q2, const Quat<T> &q3, |
||||
const T t, QuatAssumeType assumeUnit, |
||||
bool directChange) |
||||
{ |
||||
Quat<T> v0(q0), v1(q1), v2(q2), v3(q3); |
||||
if (!assumeUnit) |
||||
{ |
||||
v0 = v0.normalize(); |
||||
v1 = v1.normalize(); |
||||
v2 = v2.normalize(); |
||||
v3 = v3.normalize(); |
||||
} |
||||
|
||||
Quat<T> c0 = slerp(v0, v3, t, assumeUnit, directChange); |
||||
Quat<T> c1 = slerp(v1, v2, t, assumeUnit, directChange); |
||||
return slerp(c0, c1, 2 * t * (1 - t), assumeUnit, directChange); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::interPoint(const Quat<T> &q0, const Quat<T> &q1, |
||||
const Quat<T> &q2, QuatAssumeType assumeUnit) |
||||
{ |
||||
Quat<T> v0(q0), v1(q1), v2(q2); |
||||
if (!assumeUnit) |
||||
{ |
||||
v0 = v0.normalize(); |
||||
v1 = v1.normalize(); |
||||
v2 = v2.normalize(); |
||||
} |
||||
return v1 * cv::exp(-(cv::log(v1.conjugate() * v0, assumeUnit) + (cv::log(v1.conjugate() * v2, assumeUnit))) / 4); |
||||
} |
||||
|
||||
template <typename T> |
||||
Quat<T> Quat<T>::spline(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2, const Quat<T> &q3, const T t, QuatAssumeType assumeUnit) |
||||
{ |
||||
Quatd v0(q0), v1(q1), v2(q2), v3(q3); |
||||
if (!assumeUnit) |
||||
{ |
||||
v0 = v0.normalize(); |
||||
v1 = v1.normalize(); |
||||
v2 = v2.normalize(); |
||||
v3 = v3.normalize(); |
||||
} |
||||
T cosTheta; |
||||
std::vector<Quat<T>> vec{v0, v1, v2, v3}; |
||||
for (size_t i = 0; i < 3; ++i) |
||||
{ |
||||
cosTheta = vec[i].dot(vec[i + 1]); |
||||
if (cosTheta < 0) |
||||
{ |
||||
vec[i + 1] = -vec[i + 1]; |
||||
} |
||||
} |
||||
Quat<T> s1 = interPoint(vec[0], vec[1], vec[2], QUAT_ASSUME_UNIT); |
||||
Quat<T> s2 = interPoint(vec[1], vec[2], vec[3], QUAT_ASSUME_UNIT); |
||||
return squad(vec[1], s1, s2, vec[2], t, assumeUnit, QUAT_ASSUME_NOT_UNIT); |
||||
} |
||||
|
||||
} // namepsace
|
||||
//! @endcond
|
||||
|
||||
#endif /*OPENCV_CORE_QUATERNION_INL_HPP*/ |
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue