-
Notifications
You must be signed in to change notification settings - Fork 58
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Set lens intrinsics in Depth and Rgbd camera sensors (#390)
* Refactor projection matrix utility functions to CameraSensorUtil and set lens intrinsics in Depth and Rgbd camera sensors --------- Signed-off-by: Shameek Ganguly <[email protected]>
- Loading branch information
1 parent
67e0dea
commit bd695b2
Showing
6 changed files
with
341 additions
and
142 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,91 @@ | ||
| /* | ||
| * Copyright (C) 2023 Open Source Robotics Foundation | ||
| * | ||
| * 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. | ||
| * | ||
| */ | ||
|
|
||
| #include "CameraSensorUtil.hh" | ||
|
|
||
| #include <gz/math/Matrix4.hh> | ||
|
|
||
| ////////////////////////////////////////////////// | ||
| gz::math::Matrix4d gz::sensors::buildNDCMatrix( | ||
| double _left, double _right, | ||
| double _bottom, double _top, | ||
| double _near, double _far) | ||
| { | ||
| double inverseWidth = 1.0 / (_right - _left); | ||
| double inverseHeight = 1.0 / (_top - _bottom); | ||
| double inverseDistance = 1.0 / (_far - _near); | ||
|
|
||
| return math::Matrix4d( | ||
| 2.0 * inverseWidth, | ||
| 0.0, | ||
| 0.0, | ||
| -(_right + _left) * inverseWidth, | ||
| 0.0, | ||
| 2.0 * inverseHeight, | ||
| 0.0, | ||
| -(_top + _bottom) * inverseHeight, | ||
| 0.0, | ||
| 0.0, | ||
| -2.0 * inverseDistance, | ||
| -(_far + _near) * inverseDistance, | ||
| 0.0, | ||
| 0.0, | ||
| 0.0, | ||
| 1.0); | ||
| } | ||
|
|
||
| ////////////////////////////////////////////////// | ||
| gz::math::Matrix4d gz::sensors::buildPerspectiveMatrix( | ||
| double _intrinsicsFx, double _intrinsicsFy, | ||
| double _intrinsicsCx, double _intrinsicsCy, | ||
| double _intrinsicsS, | ||
| double _clipNear, double _clipFar) | ||
| { | ||
| return math::Matrix4d( | ||
| _intrinsicsFx, | ||
| _intrinsicsS, | ||
| -_intrinsicsCx, | ||
| 0.0, | ||
| 0.0, | ||
| _intrinsicsFy, | ||
| -_intrinsicsCy, | ||
| 0.0, | ||
| 0.0, | ||
| 0.0, | ||
| _clipNear + _clipFar, | ||
| _clipNear * _clipFar, | ||
| 0.0, | ||
| 0.0, | ||
| -1.0, | ||
| 0.0); | ||
| } | ||
|
|
||
| ////////////////////////////////////////////////// | ||
| gz::math::Matrix4d gz::sensors::buildProjectionMatrix( | ||
| double _imageWidth, double _imageHeight, | ||
| double _intrinsicsFx, double _intrinsicsFy, | ||
| double _intrinsicsCx, double _intrinsicsCy, | ||
| double _intrinsicsS, | ||
| double _clipNear, double _clipFar) | ||
| { | ||
| return buildNDCMatrix( | ||
| 0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) * | ||
| buildPerspectiveMatrix( | ||
| _intrinsicsFx, _intrinsicsFy, | ||
| _intrinsicsCx, _imageHeight - _intrinsicsCy, | ||
| _intrinsicsS, _clipNear, _clipFar); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,94 @@ | ||
| /* | ||
| * Copyright (C) 2023 Open Source Robotics Foundation | ||
| * | ||
| * 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. | ||
| * | ||
| */ | ||
|
|
||
| #ifndef GZ_SENSORS_CAMERASENSORUTIL_HH_ | ||
| #define GZ_SENSORS_CAMERASENSORUTIL_HH_ | ||
|
|
||
| #include <gz/math/Matrix4.hh> | ||
|
|
||
| #include "gz/sensors/config.hh" | ||
| #include "gz/sensors/Export.hh" | ||
|
|
||
| namespace gz | ||
| { | ||
| namespace sensors | ||
| { | ||
| // Inline bracket to help doxygen filtering. | ||
| inline namespace GZ_SENSORS_VERSION_NAMESPACE { | ||
| /// \brief Computes the OpenGL NDC matrix | ||
| /// \param[in] _left Left vertical clipping plane | ||
| /// \param[in] _right Right vertical clipping plane | ||
| /// \param[in] _bottom Bottom horizontal clipping plane | ||
| /// \param[in] _top Top horizontal clipping plane | ||
| /// \param[in] _near Distance to the nearer depth clipping plane | ||
| /// This value is negative if the plane is to be behind | ||
| /// the camera | ||
| /// \param[in] _far Distance to the farther depth clipping plane | ||
| /// This value is negative if the plane is to be behind | ||
| /// the camera | ||
| /// \return OpenGL NDC (Normalized Device Coordinates) matrix | ||
| math::Matrix4d buildNDCMatrix(double _left, double _right, double _bottom, | ||
| double _top, double _near, double _far); | ||
|
|
||
| /// \brief Computes the OpenGL perspective matrix | ||
| /// \param[in] _intrinsicsFx Horizontal focal length (in pixels) | ||
| /// \param[in] _intrinsicsFy Vertical focal length (in pixels) | ||
| /// \param[in] _intrinsicsCx X coordinate of principal point in pixels | ||
| /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels | ||
| /// \param[in] _intrinsicsS Skew coefficient defining the angle between | ||
| /// the x and y pixel axes | ||
| /// \param[in] _clipNear Distance to the nearer depth clipping plane | ||
| /// This value is negative if the plane is to be behind | ||
| /// the camera | ||
| /// \param[in] _clipFar Distance to the farther depth clipping plane | ||
| /// This value is negative if the plane is to be behind | ||
| /// the camera | ||
| /// \return OpenGL perspective matrix | ||
| math::Matrix4d buildPerspectiveMatrix( | ||
| double _intrinsicsFx, double _intrinsicsFy, double _intrinsicsCx, | ||
| double _intrinsicsCy, double _intrinsicsS, double _clipNear, | ||
| double _clipFar); | ||
|
|
||
| /// \brief Computes the OpenGL projection matrix by multiplying | ||
| /// the OpenGL Normalized Device Coordinates matrix (NDC) with | ||
| /// the OpenGL perspective matrix | ||
| /// openglProjectionMatrix = ndcMatrix * perspectiveMatrix | ||
| /// \param[in] _imageWidth Image width (in pixels) | ||
| /// \param[in] _imageHeight Image height (in pixels) | ||
| /// \param[in] _intrinsicsFx Horizontal focal length (in pixels) | ||
| /// \param[in] _intrinsicsFy Vertical focal length (in pixels) | ||
| /// \param[in] _intrinsicsCx X coordinate of principal point in pixels | ||
| /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels | ||
| /// \param[in] _intrinsicsS Skew coefficient defining the angle between | ||
| /// the x and y pixel axes | ||
| /// \param[in] _clipNear Distance to the nearer depth clipping plane | ||
| /// This value is negative if the plane is to be behind | ||
| /// the camera | ||
| /// \param[in] _clipFar Distance to the farther depth clipping plane | ||
| /// This value is negative if the plane is to be behind | ||
| /// the camera | ||
| /// \return OpenGL projection matrix | ||
| math::Matrix4d buildProjectionMatrix(double _imageWidth, double _imageHeight, | ||
| double _intrinsicsFx, double _intrinsicsFy, | ||
| double _intrinsicsCx, double _intrinsicsCy, | ||
| double _intrinsicsS, double _clipNear, | ||
| double _clipFar); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| #endif |
Oops, something went wrong.