|
| 1 | +/* |
| 2 | + * Portions Copyright (c) Meta Platforms, Inc. and affiliates. |
| 3 | + * |
| 4 | + * This source code is licensed under the MIT license found in the |
| 5 | + * LICENSE file in the root directory of this source tree. |
| 6 | + */ |
| 7 | + |
| 8 | +#include "ocean/cv/detector/FREAKDescriptor.h" |
| 9 | + |
| 10 | +namespace Ocean |
| 11 | +{ |
| 12 | + |
| 13 | +namespace CV |
| 14 | +{ |
| 15 | + |
| 16 | +namespace Detector |
| 17 | +{ |
| 18 | + |
| 19 | +FREAKDescriptor::PinholeCameraDerivativeFunctor::PinholeCameraDerivativeFunctor(const PinholeCamera& pinholeCamera, const unsigned int pyramidLevels) |
| 20 | +{ |
| 21 | + ocean_assert(pinholeCamera.isValid()); |
| 22 | + ocean_assert(pyramidLevels != 0u); |
| 23 | + |
| 24 | + cameras_.reserve(pyramidLevels); |
| 25 | + cameras_.emplace_back(pinholeCamera); |
| 26 | + |
| 27 | + unsigned int width = pinholeCamera.width(); |
| 28 | + unsigned int height = pinholeCamera.height(); |
| 29 | + |
| 30 | + for (unsigned int level = 1u; level < pyramidLevels; ++level) |
| 31 | + { |
| 32 | + width /= 2u; |
| 33 | + height /= 2u; |
| 34 | + |
| 35 | + if (width == 0u || height == 0u) |
| 36 | + { |
| 37 | + break; |
| 38 | + } |
| 39 | + |
| 40 | + cameras_.emplace_back(width, height, pinholeCamera); |
| 41 | + } |
| 42 | + |
| 43 | + cameras_.shrink_to_fit(); |
| 44 | +} |
| 45 | + |
| 46 | +FREAKDescriptor::CameraDerivativeData FREAKDescriptor::PinholeCameraDerivativeFunctor::computeCameraDerivativeData(const Eigen::Vector2f& point, const unsigned int pointPyramidLevel, float& inverseFocalLength) const |
| 47 | +{ |
| 48 | + ocean_assert(pointPyramidLevel < cameras_.size()); |
| 49 | + |
| 50 | + inverseFocalLength = float(cameras_[pointPyramidLevel].inverseFocalLengthX() + cameras_[pointPyramidLevel].inverseFocalLengthY()) * 0.5f; |
| 51 | + |
| 52 | + return computeCameraDerivativeData(cameras_[pointPyramidLevel], point); |
| 53 | +} |
| 54 | + |
| 55 | +unsigned int FREAKDescriptor::PinholeCameraDerivativeFunctor::supportedPyramidLevels() const |
| 56 | +{ |
| 57 | + return (unsigned int)(cameras_.size()); |
| 58 | +} |
| 59 | + |
| 60 | +FREAKDescriptor::CameraDerivativeData FREAKDescriptor::PinholeCameraDerivativeFunctor::computeCameraDerivativeData(const PinholeCamera& pinholeCamera, const Eigen::Vector2f& point) |
| 61 | +{ |
| 62 | + const Vector3 unprojectRayIF = pinholeCamera.vectorIF(Vector2(point.x(), point.y())); |
| 63 | + ocean_assert(Numeric::isEqualEps((Vector3((Scalar(point.x()) - pinholeCamera.principalPointX()) * pinholeCamera.inverseFocalLengthX(), (Scalar(point.y()) - pinholeCamera.principalPointY()) * pinholeCamera.inverseFocalLengthY(), 1.0f).normalized() - unprojectRayIF).length())); |
| 64 | + |
| 65 | + // TODOX Revisit this when enabling camera distortions |
| 66 | + ocean_assert(pinholeCamera.hasDistortionParameters() == false); |
| 67 | + |
| 68 | + Scalar jacobianX[3]; |
| 69 | + Scalar jacobianY[3]; |
| 70 | + Geometry::Jacobian::calculatePointJacobian2x3(jacobianX, jacobianY, pinholeCamera, HomogenousMatrix4(true), unprojectRayIF, /* distort */ false); |
| 71 | + |
| 72 | + CameraDerivativeData data; |
| 73 | + |
| 74 | + data.unprojectRayIF = Eigen::Vector3f(float(unprojectRayIF.x()), float(unprojectRayIF.y()), float(unprojectRayIF.z())); |
| 75 | + |
| 76 | + // Note: the assignment below is row-major order but Eigen memory will be column-major. I know ... |
| 77 | + data.pointJacobianMatrixIF << float(jacobianX[0]), float(jacobianX[1]), float(jacobianX[2]), float(jacobianY[0]), float(jacobianY[1]), float(jacobianY[2]); |
| 78 | + ocean_assert(data.pointJacobianMatrixIF.IsRowMajor == false); |
| 79 | + |
| 80 | + return data; |
| 81 | +} |
| 82 | + |
| 83 | +FREAKDescriptor::AnyCameraDerivativeFunctor::AnyCameraDerivativeFunctor(const SharedAnyCamera& camera, const unsigned int pyramidLevels) |
| 84 | +{ |
| 85 | + ocean_assert(camera && camera->isValid()); |
| 86 | + ocean_assert(pyramidLevels != 0u); |
| 87 | + |
| 88 | + cameras_.reserve(pyramidLevels); |
| 89 | + cameras_.emplace_back(camera); |
| 90 | + |
| 91 | + unsigned int width = camera->width(); |
| 92 | + unsigned int height = camera->height(); |
| 93 | + |
| 94 | + for (unsigned int level = 1u; level < pyramidLevels; ++level) |
| 95 | + { |
| 96 | + width /= 2u; |
| 97 | + height /= 2u; |
| 98 | + |
| 99 | + if (width == 0u || height == 0u) |
| 100 | + { |
| 101 | + break; |
| 102 | + } |
| 103 | + |
| 104 | + cameras_.emplace_back(cameras_.back()->clone(width, height)); |
| 105 | + } |
| 106 | + |
| 107 | + inverseFocalLengths_.reserve(pyramidLevels); |
| 108 | + for (const SharedAnyCamera& levelCamera : cameras_) |
| 109 | + { |
| 110 | + ocean_assert(levelCamera && levelCamera->isValid()); |
| 111 | + |
| 112 | + const float inverseFocalLength = float(levelCamera->inverseFocalLengthX() + levelCamera->inverseFocalLengthY()) * 0.5f; |
| 113 | + |
| 114 | + inverseFocalLengths_.emplace_back(inverseFocalLength); |
| 115 | + } |
| 116 | +} |
| 117 | + |
| 118 | +FREAKDescriptor::CameraDerivativeData FREAKDescriptor::AnyCameraDerivativeFunctor::computeCameraDerivativeData(const Eigen::Vector2f& point, const unsigned int pointPyramidLevel, float& inverseFocalLength) const |
| 119 | +{ |
| 120 | + ocean_assert(pointPyramidLevel < cameras_.size()); |
| 121 | + ocean_assert(cameras_.size() == inverseFocalLengths_.size()); |
| 122 | + |
| 123 | + inverseFocalLength = inverseFocalLengths_[pointPyramidLevel]; |
| 124 | + |
| 125 | + return computeCameraDerivativeData(*cameras_[pointPyramidLevel], point); |
| 126 | +} |
| 127 | + |
| 128 | +unsigned int FREAKDescriptor::AnyCameraDerivativeFunctor::supportedPyramidLevels() const |
| 129 | +{ |
| 130 | + return (unsigned int)(cameras_.size()); |
| 131 | +} |
| 132 | + |
| 133 | +FREAKDescriptor::CameraDerivativeData FREAKDescriptor::AnyCameraDerivativeFunctor::computeCameraDerivativeData(const AnyCamera& camera, const Eigen::Vector2f& point) |
| 134 | +{ |
| 135 | + const Vector3 unprojectRayIF = camera.vectorIF(Vector2(point.x(), point.y())); |
| 136 | + |
| 137 | + Scalar jacobianX[3]; |
| 138 | + Scalar jacobianY[3]; |
| 139 | + camera.pointJacobian2x3IF(unprojectRayIF, jacobianX, jacobianY); |
| 140 | + |
| 141 | + CameraDerivativeData data; |
| 142 | + |
| 143 | + data.unprojectRayIF = Eigen::Vector3f(float(unprojectRayIF.x()), float(unprojectRayIF.y()), float(unprojectRayIF.z())); |
| 144 | + |
| 145 | + // Note: the assignment below is row-major order but Eigen memory will be column-major. I know ... |
| 146 | + data.pointJacobianMatrixIF << float(jacobianX[0]), float(jacobianX[1]), float(jacobianX[2]), float(jacobianY[0]), float(jacobianY[1]), float(jacobianY[2]); |
| 147 | + ocean_assert(data.pointJacobianMatrixIF.IsRowMajor == false); |
| 148 | + |
| 149 | + return data; |
| 150 | +} |
| 151 | + |
| 152 | +} |
| 153 | + |
| 154 | +} |
| 155 | + |
| 156 | +} |
![meta-codesync[bot]](https://avatars.githubusercontent.com/in/1373969?v=4&size=40){kind=avatar}
0 commit comments