├── cmake └── pose_cov_ops_config_extras.cmake ├── .gitignore ├── colcon_defaults.yaml ├── CONTRIBUTING.md ├── .clang-tidy ├── .vscode └── settings.json ├── LICENSE ├── package.xml ├── README.md ├── test └── test_pose_cov_ops.cpp ├── include └── pose_cov_ops │ └── pose_cov_ops.h ├── src └── pose_cov_ops.cpp ├── CHANGELOG.rst └── CMakeLists.txt /cmake/pose_cov_ops_config_extras.cmake: -------------------------------------------------------------------------------- 1 | # Needed for ROS1 only: 2 | add_definitions(-DPACKAGE_ROS_VERSION=1) 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.autosave 2 | .kdev* 3 | *.kdev* 4 | *~ 5 | *.rawlog 6 | LOG_* 7 | install 8 | log 9 | build 10 | 11 | CMakeLists.txt.user* 12 | .vscode 13 | -------------------------------------------------------------------------------- /colcon_defaults.yaml: -------------------------------------------------------------------------------- 1 | build: 2 | symlink-install: true 3 | cmake-args: 4 | - "-DCMAKE_BUILD_TYPE=RelWithDebInfo" 5 | - "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" 6 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the BSD-3-clause license. 3 | 4 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 5 | line to commit messages to certify that they have the right to submit 6 | the code they are contributing to the project according to the 7 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 8 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: > 2 | -*, 3 | bugprone-*, 4 | readability-braces-around-statements, 5 | readability-else-after-return, 6 | cppcoreguidelines-interfaces-global-init, 7 | cppcoreguidelines-pro-type-member-init, 8 | cppcoreguidelines-pro-type-static-cast-downcast, 9 | cppcoreguidelines-pro-type-union-access, 10 | cppcoreguidelines-slicing, 11 | cppcoreguidelines-special-member-functions 12 | WarningsAsErrors: '' 13 | HeaderFilterRegex: '.*' 14 | FormatStyle: file 15 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "C_Cpp.autoAddFileAssociations": false, 3 | "C_Cpp.doxygen.generateOnType": false, 4 | "C_Cpp.files.exclude": { 5 | "**/.git": true, 6 | "**/.vscode": true, 7 | "**/docs": true, 8 | "**/install": true, 9 | "**/log": true 10 | }, 11 | "C_Cpp.intelliSenseEngine": "disabled", 12 | "clangd.arguments": [ 13 | "--background-index", 14 | "--compile-commands-dir=${workspaceFolder}/build", 15 | "--completion-style=detailed", 16 | "--header-insertion=never", 17 | "--pretty" 18 | ], 19 | "[cpp]": { 20 | "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd" 21 | }, 22 | "cSpell.allowCompoundWords": true, 23 | "cSpell.ignorePaths": [ 24 | "**/.git/objects/**", 25 | "**/.vscode", 26 | "**/build/", 27 | "**/install/", 28 | "**/log/", 29 | ".clang-tidy" 30 | ], 31 | "search.exclude": { 32 | "**/build": true, 33 | "**/install": true, 34 | "**/log": true 35 | }, 36 | "ros.distro": "humble" 37 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2014-2022, José Luis Blanco-Claraco, contributors. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pose_cov_ops 5 | 0.4.0 6 | C++ library for SE(2)/SE(3) pose composition operations with uncertainty 7 | 8 | Jose-Luis Blanco-Claraco 9 | Markus Bader 10 | 11 | BSD 12 | 13 | https://wiki.ros.org/pose_cov_ops 14 | 15 | 16 | ros_environment 17 | 18 | 19 | cmake 20 | catkin 21 | ament_cmake_gtest 22 | ament_cmake_gmock 23 | ament_cmake 24 | 25 | 26 | mrpt_libposes 27 | 28 | mrpt_libros_bridge 29 | 30 | cv_bridge 31 | nav_msgs 32 | sensor_msgs 33 | geometry_msgs 34 | std_msgs 35 | stereo_msgs 36 | tf2 37 | roscpp 38 | rclcpp 39 | 40 | 41 | 42 | 45 | gtest 46 | 47 | 48 | 50 | 51 | 52 | rosunit 53 | gtest 54 | 55 | 56 | ament_lint_auto 57 | ament_lint_common 58 | ament_cmake_xmllint 59 | 60 | 61 | catkin 62 | ament_cmake 63 | 64 | 65 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | | Distro | Develop branch | Releases | Stable release | 2 | | --- | --- | --- | --- | 3 | | ROS2 Humble (u22.04) | [![Build Status](https://build.ros2.org/job/Hdev__pose_cov_ops__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__pose_cov_ops__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__pose_cov_ops__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__pose_cov_ops__ubuntu_jammy_amd64__binary/) | [![Version](https://img.shields.io/ros/v/humble/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 4 | | ROS 2 Jazzy @ u24.04 | [![Build Status](https://build.ros2.org/job/Jdev__pose_cov_ops__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Jdev__pose_cov_ops__ubuntu_noble_amd64/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__pose_cov_ops__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__pose_cov_ops__ubuntu_noble_amd64__binary/) | [![Version](https://img.shields.io/ros/v/jazzy/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 5 | | ROS 2 Kilted @ u24.04 | [![Build Status](https://build.ros2.org/job/Kdev__pose_cov_ops__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Kdev__pose_cov_ops__ubuntu_noble_amd64/) | [![Build Status](https://build.ros2.org/job/Kbin_uN64__pose_cov_ops__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_uN64__pose_cov_ops__ubuntu_noble_amd64__binary/) | [![Version](https://img.shields.io/ros/v/kilted/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 6 | | ROS2 Rolling (u24.04) | [![Build Status](https://build.ros2.org/job/Rdev__pose_cov_ops__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Rdev__pose_cov_ops__ubuntu_noble_amd64/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__pose_cov_ops__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__pose_cov_ops__ubuntu_noble_amd64__binary/) | [![Version](https://img.shields.io/ros/v/rolling/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 7 | 8 | EOL distros 9 | 10 | | Distro | Last release | 11 | | --- | --- | 12 | | ROS1 Melodic (u18.04) | [![Version](https://img.shields.io/ros/v/melodic/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 13 | | ROS1 Noetic (u20.04) | [![Version](https://img.shields.io/ros/v/noetic/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 14 | | ROS2 Foxy (u20.04) | [![Version](https://img.shields.io/ros/v/foxy/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 15 | | ROS2 Galactic (u20.04) | [![Version](https://img.shields.io/ros/v/galactic/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 16 | | ROS2 Iron (u22.04) | [![Version](https://img.shields.io/ros/v/iron/pose_cov_ops)](https://index.ros.org/?search_packages=true&pkgs=pose_cov_ops) | 17 | 18 | pose_cov_ops 19 | ============ 20 | 21 | MRPT C++ library-wrapper for SE(2) and SE(3) poses and points geometric operations with uncertainty. 22 | 23 | Example ROS node projects to show how to use `pose_cov_ops` in your code: 24 | * For ROS 1: https://github.com/mrpt-ros-pkg/pose_cov_ops_example_ros1_package 25 | * For ROS 2: https://github.com/mrpt-ros-pkg/pose_cov_ops_example_ros2_package 26 | 27 | Docs: https://wiki.ros.org/pose_cov_ops 28 | 29 | -------------------------------------------------------------------------------- /test/test_pose_cov_ops.cpp: -------------------------------------------------------------------------------- 1 | /* pose_cov_ops 2 | * 3 | * Copyright 2012-2022, Jose Luis Blanco Claraco 4 | * License: BSD 3-Clause License 5 | */ 6 | 7 | #include 8 | // 9 | #include 10 | #include 11 | #include 12 | 13 | #if PACKAGE_ROS_VERSION == 1 14 | #include 15 | namespace m2r = mrpt::ros1bridge; 16 | #else 17 | #include 18 | namespace m2r = mrpt::ros2bridge; 19 | #endif 20 | 21 | TEST(PoseCovOps, composition) { 22 | using namespace std; 23 | using namespace pose_cov_ops; 24 | 25 | // Test added while debugging report: 26 | // https://github.com/mrpt-ros-pkg/pose_cov_ops/issues/7 27 | 28 | PoseWithCovariance a; 29 | a.pose.position.x = -0.333330; 30 | a.pose.position.y = 0; 31 | a.pose.position.z = 0.100000; 32 | 33 | a.pose.orientation.x = 0.707107; 34 | a.pose.orientation.y = 0; 35 | a.pose.orientation.z = 0.707107; 36 | a.pose.orientation.w = -0.000005; 37 | 38 | PoseWithCovariance b; 39 | b.pose.position.x = 1.435644; 40 | b.pose.position.y = 0; 41 | b.pose.position.z = 0; 42 | b.pose.orientation.x = 1.000000; 43 | b.pose.orientation.y = 0; 44 | b.pose.orientation.z = 0; 45 | b.pose.orientation.w = 0; 46 | 47 | PoseWithCovariance ab; 48 | pose_cov_ops::compose(a, b, ab); 49 | 50 | const mrpt::poses::CPose3D a_mrpt = m2r::fromROS(a.pose); 51 | const mrpt::poses::CPose3D b_mrpt = m2r::fromROS(b.pose); 52 | const mrpt::poses::CPose3D ab_mrpt = m2r::fromROS(ab.pose); 53 | 54 | std::cout << "a: " << a_mrpt.asString() << "\nRot:\n" 55 | << a_mrpt.getRotationMatrix() << "\n"; 56 | std::cout << "b: " << b_mrpt.asString() << "\nRot:\n" 57 | << b_mrpt.getRotationMatrix() << "\n"; 58 | std::cout << "a+b: " << ab_mrpt.asString() << "\nRot:\n" 59 | << ab_mrpt.getRotationMatrix() << "\n"; 60 | 61 | #if 0 62 | std::cout << "a: " << a << "\n"; 63 | std::cout << "b: " << b << "\n"; 64 | std::cout << "a(+)b: " << ab << "\n"; 65 | #endif 66 | 67 | EXPECT_NEAR(ab.pose.position.x, -0.3333333, 0.01); 68 | } 69 | 70 | TEST(PoseCovOps, compositionTF2) { 71 | using namespace std; 72 | using namespace pose_cov_ops; 73 | 74 | PoseWithCovariance a; 75 | a.pose.position.x = -0.333330; 76 | a.pose.position.y = 0; 77 | a.pose.position.z = 0.100000; 78 | 79 | a.pose.orientation.x = 0.707107; 80 | a.pose.orientation.y = 0; 81 | a.pose.orientation.z = 0.707107; 82 | a.pose.orientation.w = -0.000005; 83 | 84 | tf2::Transform b; 85 | b.setOrigin({1.435644, .0, .0}); 86 | b.setRotation({1.0, .0, .0, .0}); 87 | 88 | const PoseWithCovariance ab = pose_cov_ops::compose(a, b); 89 | 90 | const mrpt::poses::CPose3D a_mrpt = m2r::fromROS(a.pose); 91 | const mrpt::poses::CPose3D b_mrpt = m2r::fromROS(b); 92 | const mrpt::poses::CPose3D ab_mrpt = m2r::fromROS(ab.pose); 93 | 94 | std::cout << "a: " << a_mrpt.asString() << "\nRot:\n" 95 | << a_mrpt.getRotationMatrix() << "\n"; 96 | std::cout << "b: " << b_mrpt.asString() << "\nRot:\n" 97 | << b_mrpt.getRotationMatrix() << "\n"; 98 | std::cout << "a+b: " << ab_mrpt.asString() << "\nRot:\n" 99 | << ab_mrpt.getRotationMatrix() << "\n"; 100 | 101 | #if 0 102 | std::cout << "a: " << a << "\n"; 103 | std::cout << "b: " << b << "\n"; 104 | std::cout << "a(+)b: " << ab << "\n"; 105 | #endif 106 | 107 | EXPECT_NEAR(ab.pose.position.x, -0.3333333, 0.01); 108 | } 109 | 110 | // Run all the tests that were declared with TEST() 111 | int main(int argc, char **argv) { 112 | testing::InitGoogleTest(&argc, argv); 113 | return RUN_ALL_TESTS(); 114 | } 115 | -------------------------------------------------------------------------------- /include/pose_cov_ops/pose_cov_ops.h: -------------------------------------------------------------------------------- 1 | /* pose_cov_ops 2 | * 3 | * Copyright 2012-2025, Jose Luis Blanco Claraco 4 | * License: BSD 3-Clause License 5 | */ 6 | 7 | #pragma once 8 | 9 | #if PACKAGE_ROS_VERSION == 1 10 | #include 11 | #include 12 | #else 13 | #include 14 | #include 15 | #endif 16 | #include 17 | 18 | namespace pose_cov_ops { 19 | 20 | #if PACKAGE_ROS_VERSION == 1 21 | using Pose = geometry_msgs::Pose; 22 | using PoseWithCovariance = geometry_msgs::PoseWithCovariance; 23 | #else 24 | using Pose = geometry_msgs::msg::Pose; 25 | using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; 26 | #endif 27 | 28 | /** @name Pose composition: out = a (+) b 29 | @{ */ 30 | void compose(const Pose &a, const Pose &b, Pose &out); 31 | void compose(const PoseWithCovariance &a, const PoseWithCovariance &b, 32 | PoseWithCovariance &out); 33 | void compose(const PoseWithCovariance &a, const Pose &b, 34 | PoseWithCovariance &out); 35 | void compose(const Pose &a, const PoseWithCovariance &b, 36 | PoseWithCovariance &out); 37 | 38 | // Return-by-value versions: 39 | static inline Pose compose(const Pose &a, const Pose &b) { 40 | Pose out; 41 | compose(a, b, out); 42 | return out; 43 | } 44 | static inline PoseWithCovariance compose(const PoseWithCovariance &a, 45 | const PoseWithCovariance &b) { 46 | PoseWithCovariance out; 47 | compose(a, b, out); 48 | return out; 49 | } 50 | static inline PoseWithCovariance compose(const PoseWithCovariance &a, 51 | const Pose &b) { 52 | PoseWithCovariance out; 53 | compose(a, b, out); 54 | return out; 55 | } 56 | static inline PoseWithCovariance compose(const Pose &a, 57 | const PoseWithCovariance &b) { 58 | PoseWithCovariance out; 59 | compose(a, b, out); 60 | return out; 61 | } 62 | 63 | // Return-by-value versions using TF2 transforms: 64 | PoseWithCovariance compose(const PoseWithCovariance &a, 65 | const tf2::Transform &b); 66 | 67 | /** @} */ 68 | 69 | /** @name Pose inverse composition (a "as seen from" b): out = a (-) b 70 | @{ */ 71 | void inverseCompose(const Pose &a, const Pose &b, Pose &out); 72 | void inverseCompose(const PoseWithCovariance &a, const PoseWithCovariance &b, 73 | PoseWithCovariance &out); 74 | void inverseCompose(const PoseWithCovariance &a, const Pose &b, 75 | PoseWithCovariance &out); 76 | void inverseCompose(const Pose &a, const PoseWithCovariance &b, 77 | PoseWithCovariance &out); 78 | // Return-by-value versions: 79 | static inline Pose inverseCompose(const Pose &a, const Pose &b) { 80 | Pose out; 81 | inverseCompose(a, b, out); 82 | return out; 83 | } 84 | static inline PoseWithCovariance inverseCompose(const PoseWithCovariance &a, 85 | const PoseWithCovariance &b) { 86 | PoseWithCovariance out; 87 | inverseCompose(a, b, out); 88 | return out; 89 | } 90 | static inline PoseWithCovariance inverseCompose(const PoseWithCovariance &a, 91 | const Pose &b) { 92 | PoseWithCovariance out; 93 | inverseCompose(a, b, out); 94 | return out; 95 | } 96 | static inline PoseWithCovariance inverseCompose(const Pose &a, 97 | const PoseWithCovariance &b) { 98 | PoseWithCovariance out; 99 | inverseCompose(a, b, out); 100 | return out; 101 | } 102 | 103 | // Return-by-value versions using TF2 transforms: 104 | PoseWithCovariance inverseCompose(const PoseWithCovariance &a, 105 | const tf2::Transform &b); 106 | 107 | /** @} */ 108 | 109 | } // namespace pose_cov_ops 110 | -------------------------------------------------------------------------------- /src/pose_cov_ops.cpp: -------------------------------------------------------------------------------- 1 | /* pose_cov_ops 2 | * 3 | * Copyright 2012-2025, Jose Luis Blanco Claraco 4 | * License: BSD 3-Clause License 5 | */ 6 | 7 | #include "pose_cov_ops/pose_cov_ops.h" 8 | 9 | #include 10 | #include 11 | 12 | #if PACKAGE_ROS_VERSION == 1 13 | #include 14 | namespace m2r = mrpt::ros1bridge; 15 | #else 16 | #include 17 | namespace m2r = mrpt::ros2bridge; 18 | #endif 19 | 20 | void pose_cov_ops::compose(const Pose &a, const Pose &b, Pose &out) { 21 | 22 | out = m2r::toROS_Pose(m2r::fromROS(a) + m2r::fromROS(b)); 23 | } 24 | 25 | void pose_cov_ops::compose(const PoseWithCovariance &a, 26 | const PoseWithCovariance &b, 27 | PoseWithCovariance &out) { 28 | using namespace mrpt::poses; 29 | 30 | const CPose3DPDFGaussian A = m2r::fromROS(a); 31 | const CPose3DPDFGaussian B = m2r::fromROS(b); 32 | 33 | const CPose3DPDFGaussian OUT = A + B; 34 | out = m2r::toROS_Pose(OUT); 35 | } 36 | 37 | void pose_cov_ops::compose(const PoseWithCovariance &a, const Pose &b, 38 | PoseWithCovariance &out) { 39 | using namespace mrpt::poses; 40 | 41 | CPose3DPDFGaussian A = m2r::fromROS(a); 42 | const CPose3D B = m2r::fromROS(b); 43 | 44 | A += B; 45 | out = m2r::toROS_Pose(A); 46 | } 47 | 48 | void pose_cov_ops::compose(const Pose &a, const PoseWithCovariance &b, 49 | PoseWithCovariance &out) { 50 | using namespace mrpt::poses; 51 | 52 | const CPose3D A = m2r::fromROS(a); 53 | CPose3DPDFGaussian B = m2r::fromROS(b); 54 | 55 | B.changeCoordinatesReference(A); // b = a (+) b 56 | out = m2r::toROS_Pose(B); 57 | } 58 | 59 | pose_cov_ops::PoseWithCovariance 60 | pose_cov_ops::compose(const pose_cov_ops::PoseWithCovariance &a, 61 | const tf2::Transform &b) { 62 | using namespace mrpt::poses; 63 | 64 | CPose3DPDFGaussian A = m2r::fromROS(a); 65 | const CPose3D B = m2r::fromROS(b); 66 | 67 | A += B; 68 | return m2r::toROS_Pose(A); 69 | } 70 | 71 | void pose_cov_ops::inverseCompose(const Pose &a, const Pose &b, Pose &out) { 72 | out = m2r::toROS_Pose(m2r::fromROS(a) - m2r::fromROS(b)); 73 | } 74 | 75 | void pose_cov_ops::inverseCompose(const PoseWithCovariance &a, 76 | const PoseWithCovariance &b, 77 | PoseWithCovariance &out) { 78 | using namespace mrpt::poses; 79 | 80 | const CPose3DPDFGaussian A = m2r::fromROS(a); 81 | const CPose3DPDFGaussian B = m2r::fromROS(b); 82 | 83 | const CPose3DPDFGaussian OUT = A - B; 84 | out = m2r::toROS_Pose(OUT); 85 | } 86 | void pose_cov_ops::inverseCompose(const PoseWithCovariance &a, const Pose &b, 87 | PoseWithCovariance &out) { 88 | using namespace mrpt::poses; 89 | using namespace mrpt::math; 90 | 91 | const CPose3DPDFGaussian A = m2r::fromROS(a); 92 | const CPose3D B_mean = m2r::fromROS(b); 93 | 94 | const CPose3DPDFGaussian B(B_mean, CMatrixDouble66::Zero()); 95 | 96 | const CPose3DPDFGaussian OUT = A - B; 97 | out = m2r::toROS_Pose(OUT); 98 | } 99 | 100 | void pose_cov_ops::inverseCompose(const Pose &a, const PoseWithCovariance &b, 101 | PoseWithCovariance &out) { 102 | using namespace mrpt::poses; 103 | using namespace mrpt::math; 104 | 105 | const CPose3D A_mean = m2r::fromROS(a); 106 | const CPose3DPDFGaussian B = m2r::fromROS(b); 107 | 108 | const CPose3DPDFGaussian A(A_mean, CMatrixDouble66::Zero()); 109 | 110 | const CPose3DPDFGaussian OUT = A - B; 111 | out = m2r::toROS_Pose(OUT); 112 | } 113 | 114 | pose_cov_ops::PoseWithCovariance 115 | pose_cov_ops::inverseCompose(const pose_cov_ops::PoseWithCovariance &a, 116 | const tf2::Transform &b) { 117 | using namespace mrpt::poses; 118 | using namespace mrpt::math; 119 | 120 | const CPose3DPDFGaussian A = m2r::fromROS(a); 121 | const CPose3D B_mean = m2r::fromROS(b); 122 | 123 | const CPose3DPDFGaussian B(B_mean, CMatrixDouble66::Zero()); 124 | 125 | const CPose3DPDFGaussian OUT = A - B; 126 | return m2r::toROS_Pose(OUT); 127 | } 128 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pose_cov_ops 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2025-07-07) 6 | ------------------ 7 | * Update copyright year 8 | * FIX: Build error against latest tf2 9 | * README: Update EOL distros 10 | * Better integration with colcon and clang-tidy 11 | * Contributors: Jose Luis Blanco-Claraco 12 | 13 | 0.3.14 (2025-05-28) 14 | ------------------- 15 | * Update README with Kilted badges 16 | * Fix: Remove usage of obsolete ament_target_dependencies() 17 | * Contributors: Jose Luis Blanco-Claraco 18 | 19 | 0.3.13 (2024-10-26) 20 | ------------------- 21 | * Update README.md to add links to example ROS1/ROS2 packages. 22 | * FIX: Missing CMake export symbol for ROS1 23 | * Contributors: Jose Luis Blanco-Claraco 24 | 25 | 0.3.12 (2024-08-27) 26 | ------------------- 27 | * Depend on new mrpt_lib packages (deprecate mrpt2) 28 | * Contributors: Jose Luis Blanco-Claraco 29 | 30 | 0.3.11 (2023-09-18) 31 | ------------------- 32 | * Fix ROS 1 warning: catkin_add_gtest() should be inside CATKIN_ENABLE_TESTING 33 | * Fix typos 34 | * Contributors: Jose Luis Blanco-Claraco 35 | 36 | 0.3.10 (2023-04-07) 37 | ------------------- 38 | * Fix installation of headers for ROS 1 39 | * Contributors: Jose Luis Blanco Claraco 40 | 41 | 0.3.9 (2023-04-01) 42 | ------------------ 43 | * Merge pull request `#12 `_ from roncapat/patch-1 44 | Fix build issues with ROS2 45 | * Update README.md: fix badge table 46 | * Contributors: Jose Luis Blanco-Claraco, Patrick Roncagliolo 47 | 48 | 0.3.8 (2022-09-06) 49 | ------------------ 50 | * Add compose() and inverseCompose() versions for tf2::Transform 51 | * Contributors: Jose Luis Blanco Claraco 52 | 53 | 0.3.7 (2022-06-24) 54 | ------------------ 55 | * Remove duplicated entry in package.xml 56 | * Offer an C++ API with return-by-value semantics. 57 | * Contributors: Jose Luis Blanco-Claraco 58 | 59 | 0.3.6 (2022-06-12) 60 | ------------------ 61 | * fix package.xml build_type for ros1/ros2 62 | * Contributors: Jose Luis Blanco Claraco 63 | 64 | 0.3.5 (2022-06-10) 65 | ------------------ 66 | * Remove find_package() calls due to wrong mrpt-ros2bridge-config.cmake in older mrpt2 versions 67 | * Contributors: Jose Luis Blanco-Claraco 68 | 69 | 0.3.4 (2022-05-31) 70 | ------------------ 71 | * Fix missing cmake xmllint at configure time. 72 | * Contributors: Jose Luis Blanco-Claraco 73 | 74 | 0.3.3 (2022-05-30) 75 | ------------------ 76 | * Fix build for ros2 77 | * Rename license as "LICENSE" 78 | * Add contributing.md file 79 | * Contributors: Jose Luis Blanco-Claraco 80 | 81 | 0.3.2 (2022-05-10) 82 | ------------------ 83 | * Unify ros1/ros2 build system in one single git branch. 84 | * Port to mrpt2, avoid dependency on pkg mrpt_bridge 85 | * Contributors: Jose Luis Blanco Claraco 86 | 87 | 0.3.1 (2022-05-04) 88 | ------------------ 89 | * Add License file 90 | * remove wrong xml comment reference to mrpt1 91 | * remove travis CI 92 | * Update README.md 93 | * Contributors: Jose Luis Blanco-Claraco 94 | 95 | 0.3.0 (2022-03-04) 96 | ------------------ 97 | * Update URLs to https 98 | * Update build dep to mrpt2 99 | * update to mrpt2 100 | * add basic unit test 101 | * Set message to status again instead of warning 102 | * Updating CMakeLists.txt to pass build on Ubuntu 16.04LTS with CMake 3.5 with the latest version of MRPT 103 | * Add ros build farm badgets and links 104 | * Contributors: Inounx, Jose Luis Blanco-Claraco, Julian Lopez Velasquez 105 | 106 | 0.2.1 (2018-05-21) 107 | ------------------ 108 | * fix catkin_lint issues 109 | * Contributors: Jose Luis Blanco Claraco 110 | 111 | 0.2.0 (2018-01-09) 112 | ------------------ 113 | * Made compatible with MRPT 1.x and 2.x 114 | * Contributors: Jose Luis Blanco Claraco 115 | 116 | 0.1.7 (2017-12-09) 117 | ------------------ 118 | * relaxing need to find lib mrpt >=1.5 119 | * Silent GCC warnings with Eigen hdrs 120 | * Contributors: Jose Luis Blanco Claraco 121 | 122 | 0.1.6 (2017-12-04) 123 | ------------------ 124 | * Fix catkin dependencies 125 | * Use C++14 126 | * Fix build against MRPT 1.5 and 2.0 127 | * Contributors: Javier G. Monroy, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco 128 | 129 | 0.1.5 (2014-12-23) 130 | ------------------ 131 | 132 | 0.1.4 (2014-12-23) 133 | ------------------ 134 | * Ported from old repository in mrpt_navigation 135 | 136 | 0.1.3 (2014-12-18) 137 | ------------------ 138 | * Fix many missing install files 139 | * Contributors: Jose Luis Blanco 140 | 141 | 0.1.2 (2014-12-18) 142 | ------------------ 143 | 144 | 0.1.1 (2014-12-17) 145 | ------------------ 146 | * First public binary release. 147 | 148 | 0.1.0 (2014-12-17) 149 | ------------------ 150 | * consistent version numbers 151 | * Fixed: pose_cov_ops needs export section in manifest 152 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | execute_process(COMMAND bash -c "env | grep ROS" OUTPUT_VARIABLE ENV_VAL) 2 | message(STATUS "Env variables:\n${ENV_VAL}") 3 | if ("$ENV{ROS_VERSION}" STREQUAL "2") 4 | cmake_minimum_required(VERSION 3.8) 5 | set(PACKAGE_ROS_VERSION 2) 6 | set(ROS_2 TRUE) 7 | else() 8 | cmake_minimum_required(VERSION 3.1) 9 | set(PACKAGE_ROS_VERSION 1) 10 | set(ROS_1 TRUE) 11 | endif() 12 | 13 | project(pose_cov_ops) 14 | 15 | # ----------------------------------------- 16 | # ROS 1 17 | # ----------------------------------------- 18 | if (ROS_1) 19 | ## Find catkin macros and libraries 20 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 21 | ## is used, also find other catkin packages 22 | find_package(catkin REQUIRED COMPONENTS 23 | geometry_msgs 24 | tf2 25 | roscpp 26 | ) 27 | 28 | ################################### 29 | ## catkin specific configuration ## 30 | ################################### 31 | ## The catkin_package macro generates cmake config files for your package 32 | ## Declare things to be passed to dependent projects 33 | ## INCLUDE_DIRS: uncomment this if you package contains header files 34 | ## LIBRARIES: libraries you create in this project that dependent projects also need 35 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 36 | ## DEPENDS: system dependencies of this project that dependent projects also need 37 | catkin_package( 38 | INCLUDE_DIRS include 39 | LIBRARIES ${PROJECT_NAME} 40 | CATKIN_DEPENDS 41 | geometry_msgs 42 | tf2 43 | # Export the macro to other packages 44 | CFG_EXTRAS pose_cov_ops_config_extras.cmake 45 | ) 46 | 47 | find_package(mrpt-ros1bridge REQUIRED) 48 | endif() 49 | 50 | # ----------------------------------------- 51 | # ROS 2 52 | # ----------------------------------------- 53 | if (ROS_2) 54 | # find dependencies 55 | find_package(ament_cmake REQUIRED) 56 | 57 | find_package(geometry_msgs REQUIRED) 58 | find_package(tf2 REQUIRED) 59 | find_package(mrpt-ros2bridge REQUIRED) 60 | endif() 61 | 62 | # ----------------------------------------- 63 | # ROS 1 & 2 common deps: 64 | # ----------------------------------------- 65 | find_package(mrpt-poses REQUIRED) 66 | 67 | ########### 68 | ## Build ## 69 | ########### 70 | 71 | # Specify additional locations of header files 72 | # Your package locations should be listed before other locations 73 | if (catkin_INCLUDE_DIRS) 74 | include_directories(include ${catkin_INCLUDE_DIRS}) 75 | endif() 76 | 77 | ## Declare a cpp library 78 | add_library(${PROJECT_NAME} SHARED 79 | src/${PROJECT_NAME}.cpp 80 | ) 81 | 82 | # Done automatically via mrpt exported cmake flags 83 | # set(CMAKE_CXX_STANDARD 17) 84 | 85 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 86 | # High level of warnings. 87 | target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic) 88 | 89 | # The -Wno-long-long is required in 64bit systems when including sytem headers. 90 | # The -Wno-variadic-macros was needed for Eigen3, StdVector.h 91 | target_compile_options(${PROJECT_NAME} PUBLIC -Wno-long-long -Wno-variadic-macros) 92 | # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 93 | if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") 94 | target_compile_options(${PROJECT_NAME} PUBLIC -Wno-ignored-attributes -Wno-int-in-bool-context) 95 | endif() 96 | endif() 97 | 98 | ## Declare a cpp executable 99 | # add_executable(pose_cov_ops_node src/pose_cov_ops_node.cpp) 100 | 101 | ## Add cmake target dependencies of the executable/library 102 | ## as an example, message headers may need to be generated before nodes 103 | # add_dependencies(pose_cov_ops_node pose_cov_ops_generate_messages_cpp) 104 | 105 | if (ROS_1) 106 | target_link_libraries(${PROJECT_NAME} 107 | PRIVATE 108 | ${catkin_LIBRARIES} 109 | mrpt::ros1bridge 110 | mrpt::poses 111 | ) 112 | else() 113 | target_link_libraries(${PROJECT_NAME} 114 | mrpt::ros2bridge 115 | mrpt::poses 116 | rclcpp::rclcpp 117 | tf2::tf2 118 | ${geometry_msgs_TARGETS} 119 | ) 120 | endif() 121 | 122 | 123 | target_include_directories(${PROJECT_NAME} PUBLIC 124 | "$" 125 | "$" 126 | ) 127 | 128 | # This does not seem to work with ROS1 (that's the special catkin CFG_EXTRAS above) 129 | target_compile_definitions(${PROJECT_NAME} PUBLIC 130 | PACKAGE_ROS_VERSION=${PACKAGE_ROS_VERSION} 131 | ) 132 | 133 | ############# 134 | ## Install ## 135 | ############# 136 | 137 | # all install targets should use catkin DESTINATION variables 138 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 139 | 140 | ## Mark executable scripts (Python etc.) for installation 141 | # in contrast to setup.py, you can choose the destination 142 | # install(PROGRAMS 143 | # scripts/my_python_script 144 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 145 | # ) 146 | 147 | # Mark executables and/or libraries for installation 148 | if (ROS_2) 149 | set(CATKIN_PACKAGE_LIB_DESTINATION lib) 150 | set(CATKIN_PACKAGE_BIN_DESTINATION bin) 151 | set(CATKIN_PACKAGE_INCLUDE_DESTINATION include) 152 | endif() 153 | 154 | install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} 155 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 156 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 157 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 158 | ) 159 | 160 | if(ROS_2) 161 | set(INCLUDE_DIR_TO_INSTALL "include/${PROJECT_NAME}") 162 | else() 163 | set(INCLUDE_DIR_TO_INSTALL "include/${PROJECT_NAME}/") 164 | endif() 165 | 166 | # Mark cpp header files for installation 167 | install(DIRECTORY ${INCLUDE_DIR_TO_INSTALL} 168 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 169 | FILES_MATCHING PATTERN "*.h" 170 | ) 171 | 172 | ############# 173 | ## Testing ## 174 | ############# 175 | # Add gtest based cpp test target and link libraries 176 | if (ROS_1) 177 | if(CATKIN_ENABLE_TESTING) 178 | catkin_add_gtest(${PROJECT_NAME}-test test/test_pose_cov_ops.cpp) 179 | endif() 180 | endif() 181 | 182 | if (ROS_2) 183 | #find_package(ament_cmake_copyright REQUIRED) 184 | #find_package(ament_cmake_cppcheck REQUIRED) 185 | #find_package(ament_cmake_cpplint REQUIRED) 186 | #find_package(ament_cmake_lint_cmake REQUIRED) 187 | #find_package(ament_cmake_uncrustify REQUIRED) 188 | find_package(ament_cmake_xmllint REQUIRED) 189 | 190 | #ament_copyright() 191 | #ament_cpplint() 192 | #ament_lint_cmake() 193 | ament_xmllint() 194 | 195 | find_package(ament_cmake_gtest REQUIRED) 196 | 197 | ament_add_gtest(${PROJECT_NAME}-test test/test_pose_cov_ops.cpp) 198 | 199 | ament_export_dependencies(mrpt-ros2bridge) 200 | 201 | # Export old-style CMake variables 202 | ament_export_include_directories("include/${PROJECT_NAME}") 203 | ament_export_libraries(${PROJECT_NAME}) 204 | 205 | # Export modern CMake targets 206 | ament_export_targets(export_${PROJECT_NAME}) 207 | 208 | ament_package() 209 | endif() 210 | 211 | if(TARGET ${PROJECT_NAME}-test) 212 | target_link_libraries(${PROJECT_NAME}-test 213 | ${PROJECT_NAME} 214 | mrpt::poses 215 | ) 216 | if (ROS_1) 217 | target_link_libraries(${PROJECT_NAME}-test mrpt::ros1bridge) 218 | else() 219 | target_link_libraries(${PROJECT_NAME}-test mrpt::ros2bridge) 220 | endif() 221 | endif() 222 | --------------------------------------------------------------------------------