From 8376fc20e76e468313f940df8e65f19587554d09 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 17 Mar 2026 14:10:01 +0000 Subject: [PATCH 01/31] Adds pipx to apt install for Dockerfile --- Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 1c4f73f5..c2e6acb8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -17,7 +17,8 @@ RUN apt-get update && \ python3-scipy \ python3-transforms3d \ python3-serial \ - libusb-dev + libusb-dev \ + pipx RUN echo "ALL ALL = (ALL) NOPASSWD: ALL" >> /etc/sudoers && \ useradd -m -s /bin/bash -G sudo ubuntu && \ From c8fd79402cd8c3f91708146e097051f9c34c05e2 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 17 Mar 2026 14:10:25 +0000 Subject: [PATCH 02/31] Changes way to find source for "libusb.h" for the IMU node --- mrobosub_hal_imu/CMakeLists.txt | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/mrobosub_hal_imu/CMakeLists.txt b/mrobosub_hal_imu/CMakeLists.txt index 78f46bda..725cd9ce 100644 --- a/mrobosub_hal_imu/CMakeLists.txt +++ b/mrobosub_hal_imu/CMakeLists.txt @@ -1,13 +1,15 @@ cmake_minimum_required(VERSION 3.8) project(mrobosub_hal_imu) +find_package(PkgConfig REQUIRED) +pkg_check_modules(LIBUSB REQUIRED libusb-1.0) + add_subdirectory(inertial-sense-sdk) include_directories( inertial-sense-sdk/src - inertial-sense-sdk/src/libusb/libusb - inertial-sense-sdk/src/libusb/linux inertial-sense-sdk/src/yaml-cpp + ${LIBUSB_INCLUDE_DIRS} ) # find dependencies @@ -20,7 +22,7 @@ find_package(mrobosub_msgs REQUIRED) add_executable(imu_node src/imu_node.cpp) ament_target_dependencies(imu_node rclcpp mrobosub_msgs) -target_link_libraries(imu_node InertialSenseSDK) +target_link_libraries(imu_node InertialSenseSDK ${LIBUSB_LIBRARIES}) install(TARGETS imu_node From 109bd4287feacd91c0e54b728518ca49f3be48eb Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 17 Mar 2026 14:10:49 +0000 Subject: [PATCH 03/31] Creates new localization node (to be migration of TURTLMap) --- mrobosub_localization_cpp/CMakeLists.txt | 35 +++++++++++++++++++ mrobosub_localization_cpp/LICENSE | 30 ++++++++++++++++ mrobosub_localization_cpp/package.xml | 18 ++++++++++ .../src/localization.cpp | 10 ++++++ 4 files changed, 93 insertions(+) create mode 100644 mrobosub_localization_cpp/CMakeLists.txt create mode 100644 mrobosub_localization_cpp/LICENSE create mode 100644 mrobosub_localization_cpp/package.xml create mode 100644 mrobosub_localization_cpp/src/localization.cpp diff --git a/mrobosub_localization_cpp/CMakeLists.txt b/mrobosub_localization_cpp/CMakeLists.txt new file mode 100644 index 00000000..3336de18 --- /dev/null +++ b/mrobosub_localization_cpp/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(mrobosub_localization_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(localization src/localization.cpp) +target_include_directories(localization PUBLIC + $ + $) +target_compile_features(localization PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS localization + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/mrobosub_localization_cpp/LICENSE b/mrobosub_localization_cpp/LICENSE new file mode 100644 index 00000000..24d43da7 --- /dev/null +++ b/mrobosub_localization_cpp/LICENSE @@ -0,0 +1,30 @@ +All rights reserved. + +Software License Agreement (BSD License 2.0) + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * 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. + * 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 OWNER 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. diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml new file mode 100644 index 00000000..e6ecd1c6 --- /dev/null +++ b/mrobosub_localization_cpp/package.xml @@ -0,0 +1,18 @@ + + + + mrobosub_localization_cpp + 0.0.0 + TODO: Package description + ubuntu + BSD-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mrobosub_localization_cpp/src/localization.cpp b/mrobosub_localization_cpp/src/localization.cpp new file mode 100644 index 00000000..7c38a519 --- /dev/null +++ b/mrobosub_localization_cpp/src/localization.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world mrobosub_localization_cpp package\n"); + return 0; +} From b7e8c2b0a6663629f49bb21ddfe912000d9f90ff Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 17 Mar 2026 15:23:43 +0000 Subject: [PATCH 04/31] Change license to reflect Apache-2.0 from TURTLMap --- mrobosub_localization_cpp/LICENSE | 231 ++++++++++++++++++++++---- mrobosub_localization_cpp/package.xml | 2 +- 2 files changed, 202 insertions(+), 31 deletions(-) diff --git a/mrobosub_localization_cpp/LICENSE b/mrobosub_localization_cpp/LICENSE index 24d43da7..8fa7a5de 100644 --- a/mrobosub_localization_cpp/LICENSE +++ b/mrobosub_localization_cpp/LICENSE @@ -1,30 +1,201 @@ -All rights reserved. - -Software License Agreement (BSD License 2.0) - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * 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. - * 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 OWNER 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. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2026 University of Michigan Field Robotics + + 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. \ No newline at end of file diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml index e6ecd1c6..c9637985 100644 --- a/mrobosub_localization_cpp/package.xml +++ b/mrobosub_localization_cpp/package.xml @@ -5,7 +5,7 @@ 0.0.0 TODO: Package description ubuntu - BSD-2.0 + Apache-2.0 ament_cmake From 7cce5e450f81bae9987d5d27e2c2c18275e0630e Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 18:31:17 +0000 Subject: [PATCH 05/31] Creates header files from TURTLMap repo --- .../BluerovBarometerFactor.h | 18 +++ .../mrobosub_localization_cpp/DvlOnlyFactor.h | 26 ++++ .../mrobosub_localization_cpp/Parameters.h | 87 +++++++++++ .../mrobosub_localization_cpp/Posegraph.h | 135 ++++++++++++++++++ .../mrobosub_localization_cpp/PosegraphNode.h | 94 ++++++++++++ .../PreintegratedVelocityHelpers.h | 84 +++++++++++ 6 files changed, 444 insertions(+) create mode 100644 mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h create mode 100644 mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h create mode 100644 mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h create mode 100644 mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h create mode 100644 mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h create mode 100644 mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h new file mode 100644 index 00000000..cf883c4f --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -0,0 +1,18 @@ +#ifndef __BLUEROV_BAROMETER_FACTOR_H__ +#define __BLUEROV_BAROMETER_FACTOR_H__ + +namespace localization { +class BluerovBarometerFactor /*: public NoiseModelFactor1 */ { +private: + double _measured; + +public: + BluerovBarometerFactor(); + virtual ~BluerovBarometerFactor(); + BluerovBarometerFactor(int /*gtsam::Key*/ key, double measured, const int /*gtsam::SharedNoiseModle*/ &model); + int /*gtsam::Vector*/ evaluate_error(const int /*gtsam::Pose3*/ &pose, int /*boost::optional*/ H) const; +}; +} // namespace localization + + +#endif //__BLUEROV_BAROMETER_FACTOR_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h new file mode 100644 index 00000000..39ad3a2f --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h @@ -0,0 +1,26 @@ +#ifndef __DVL_ONLY_FACTOR_H__ +#define __DVL_ONLY_FACTOR_H__ + +#include "PreintegratedVelocityHelpers.h" + +class DvlOnlyFactor /* : public gtsam::NoiseModelFactorN*/ { +public: // Methods + DvlOnlyFactor(); + DVLOnlyFactor(int /*gtsam::Key*/ pose_i, int /*gtsam::Key*/ pose_j, int /*gtsam::Key*/ vbias_i, + const PreintegratedVelocityMeasurementsDvlOnly &pvm); + virtual ~DvlOnlyFactor(); + + int /*gtsam::Vector*/ evaluate_error( + const int /*gtsam::Pose3*/ &pose_i, + const int /*gtsam::Pose3*/ &pose_j, + const int /*gtsam::imuBias::ConstantBias*/ &vbias_i, + int /*boost::optional*/ H1, + int /*boost::optional*/ H2, + int /*boost::optional*/ H3 + ) const; + +private: // Members + PreintegratedVelocityMeasurementsDvlOnly _pvm; +}; + +#endif //__DVL_ONLY_FACTOR_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h new file mode 100644 index 00000000..6befa0d0 --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h @@ -0,0 +1,87 @@ +#ifndef __PARAMETERS_H__ +#define __PARAMETERS_H__ + +namespace localization { +struct SensorList { + bool is_dvl_used; + bool is_imu_used; + bool is_barometer_used; + bool is_sonar_used; + bool are_cams_used; +}; + +struct ImuParameters { + double acc_max; + double gyro_max; + double gyro_noise_density; + double acc_noise_density; + double acc_random_walk; + double gyro_random_walk; + double acc_bias_prior; + double gyro_bias_prior; + double imu_rate; + double integration_covariance; + double g; + double dvl_bias_prior; + double dt_imu; +}; + +struct SensorTopics { + std::string imu_topic; + std::string dvl_topic; + std::string barometer_topic; + std::string sonar_topic; + std::string dvl_local_position_topic; +}; + +struct Extrinsics { + int /*Eigen::Matrix4d */ T_SD; // dvl to imu + int /*Eigen::Matrix4d */ T_SSo; // sonar to imu + int /*Eigen::Matrix4d */ T_BS; // imu to body + int /*Eigen::Matrix4d */ T_SBa; // barometer to imu + int /*Eigen::Matrix4d */ T_W_WD; // world to dvl world +}; + +struct OptimizationParameters { + double lambda_upper_bound; + double lambda_lower_bound; + double lambda_initial; + int max_iterations; + double relative_error_tolerance; + double absolute_error_tolerance; +}; + +struct ImuPreintegrationParameters { + double gap_time; +}; + +class Parameters { +public: + Parameters(); + ~Parameters(); + + ImuParameters _imu_params; + Extrinsics _extrinsics; + SensorList _sensor_list; + SensorTopics _sensor_topics; + OptimizationParameters _optimization_params; + ImuPreintegrationParams _imu_preintegration_params; + + std::vector _rosbag_topics; + + // TODO: Can we remove this? + bool _using_orbslam; + + // TODO: Parse these into separate parameter classes. + int _num_iters; + double _baro_atm_pressure; + bool _using_smoother; + double _keyframe_gap_time; + double _dvl_fom_threshold; + bool _using_pseudo_dvl; +} + + +} // namespace localization + +#endif //__PARAMETERS_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h new file mode 100644 index 00000000..5094b519 --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -0,0 +1,135 @@ +#ifndef __POSEGRAPH_H__ +#define __POSEGRAPH_H__ +#include +#include +#include + +namespace localization { +class Posegraph { +public: // Members + int _index; + + // Smoother + int /* gtsam::ISAM2Params */ _smoother_parameters; + double _smoother_lag = 6.0; // TODO + int /* gtsam::BatchFixedLagSmoother */ _smoother_ISAM2; + int /* gtsam::FixedLagSmoother::KeyTimestampMap */ _smoother_timestamps; + + // GTSAM + int /*gtsam::NonlinearFactorGraph*/ *_graph; + int /*gtsam::Values*/ *_initial; + int /*gtsam::Values*/ *_result; + int /*gtsam::PreintegratedCombinedMeasurements*/ *_preintegrated_measurements; // used to be pim + // BlueRovPreintegratedVelocityMeasurements*/ *_pvm; + int /*PreintegratedVelocityMeasurementsDvlOnly*/ *_preintegrated_velocity_measurements; // used to be pvm + int /*gtsam::PreintegratedCombinedMeasurements::Params*/ *_preintergrated_measurement_params; // used to be pim_params + int /*PoseGraphParameters*/ *_pose_graph_params; // use to be params + + // Prior imuBias + int /*gtsam::imuBias::ConstantBias*/ _prior_imu_bias; + int /*gtsam::imuBias::ConstantBias*/ _prior_dvl_bias; // TODO: = gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0)); + + // Transforms + int /*gtsam::Matrix44*/ _T_SD; // sensor (IMU) to DVL affine transform + int /*gtsam::Matrix44*/ _T_SB; // sensor (IMU)to robot center affine transform + int /*gtsam::Matrix44*/ _T_W_WD; // world to DVL world transform + + // Previous + int /*gtsam::Pose3*/ _prev_pose; + int /*gtsam::Vector3*/ _prev_vel; + int /*gtsam::NavState*/ _prev_state; + double _prev_keyframe_time; // OLD TODO: used for saving the previous keyframe time + + // Current + int /*gtsam::Pose3*/ _current_pose; + int /*gtsam::Vector3*/ _current_vel; + int /*gtsam::NavState*/ _current_state; + int /*gtsam::imuBias::ConstantBias*/ _current_imu_bias; + double _current_time; + + std::vector _current_dvl_vels; + std::vector _current_dvl_poses; + std::vector _current_dvl_rotations; + std::vector _current_dvl_foms; // TODO: What does FOM mean? + std::vector _imu_rot_list; + int /*gtsam::Rot3*/ _imu_prev_rot; + + std::vector _current_dvl_timestamps; + std::vector _current_dvl_local_timestamps; + +private: // Members + std::mt19937 _rng; + std::normal_distribution<> _normal_distribution; + double _visual_gap_time; + + double _W_measurement_z; + /* gtsam::Vector3 */ double _B_accelerometer_S; + /* gtsam::Vector3 */ double _B_gyroscope_S; + /* gtsam::Vector3 */ double _B_velocity_D; + /* gtsam::Vector3 */ double _B_position_C; + +public: // Methods + // Ctors + Dtors + AUVPoseGraph(); + AUVPoseGraph(std::string &config_file) + ~AUVPoseGraph(); + + // Depth factors + void add_depth_factor(); + + // IMU factors + void add_imu_factor(); + void set_imu_params(); + + // DVL Factors + void add_velocity_factor(); + void add_velocity_factor_with_rotation_interpolation(bool using_slerp); + void add_dvl_factor(bool using_slerp); + void add_dvl_factor_imu_rotation(); + void add_dvl_odometry_factor(double noise); + + // DVL Suppliers + void add_dvl_velocity(double time_stamp /* gtsam::Vector3 velocity */); + void add_dvl_pose(double time_stamp /* gtsam::Pose3 pose*/); + void add_dvl_rotation(double time_stamp /* gtsam::Rot3 rotation */); + + // Other factors + void add_visual_constraint_factor(/*gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx*/); + void add_sonar_factor(); + void add_prior_factor(/*gtsam::Pose3 initial_pose, gtsam::Vector initial_vec, double pose_noise*/); + + // Creating transforms + void define_transforms(); + + // Adding estimates + void add_simple_estimate(double dt, double noise); + void add_initial_estimate(/*gtsam::Pose3 initial_pose, gtsam::Vector3 intitial_vel*/); + void add_visual_estimate(const std::vector vertex); + + // Working with the pose graph + void initialize_pose_graph(); + void initialize_pose_graph_from_imu(/*gtsam::Rot3 initial_rotation*/); + void optimize_pose_graph(); + void optimize_pose_graph_smoother(); + void add_edges_to_graph(const std::vector> &edges); + + // Getters + double get_depth_measurement(); + void /* gtsam::Vector3 */ get_accelerometer_measurement(); + void /* gtsam::Vector3 */ get_gyroscope_measurement(); + void /* gtsam::Vector3 */ get_velocity_measurement(); + void /* gtsam::Vector3 */ get_position_measurement(); + double get_visual_gap_time(); + void /* gtsam::Rot3 */ find_current_pose_for_dvl_vel(double time_stamp); + + // Setters + void set_depth_measurement(double W_measurement_z); + void set_accelerometer_measurement(/*gtsam::Vector3*/ B_accelerometer_S); + void set_gyroscope_measurement(/*gtsam::Vector3*/ B_gyroscope_S); + void set_velocity_measurement(/*gtsam::Vector3*/ B_velocity_D); + void set_position_measurement(/*gtsam::Vector3*/ W_position_C); + void set_visual_gap_time(double visualGapTime); +} +} // namespace localization + +#endif //__POSEGRAPH_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h new file mode 100644 index 00000000..7bc5b5f7 --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h @@ -0,0 +1,94 @@ +#ifndef __POSEGRAPH_H__ +#define __POSEGRAPH_H__ + +#include "Posegraph.h" +#include "Parameters.h" +#include "PreintegratedVelocityHelpers.h" +#include "DvlOnlyFactor.h" +#include "BluerovBarometerFactor.h" + +namespace localization { +class PosegraphNode { +public: + PosegraphNode(); + PosegraphNode(std::string config_file); + ~PosegraphNode(); + +private: // Members + Posegraph *_posegraph; + + // Define subscribers + int /*ros::Subscriber*/ _imu_sub; + int /*ros::Subscriber*/ _dvl_sub; + int /*ros::Subscriber*/ _baro_sub; + int /*ros::Subscriber*/ _dvl_local_sub; + + // Define publishers + int /*ros::Publisher*/ _pose_pub; + int /*ros::Publisher*/ _dvl_local_pose_pub; + int /*ros::Publisher*/ _path_pub; + int /*nav_msgs::Path*/ _path_msg; + + // TODO: I think these might have to do with the MultiLevelExecutors and stuff like that. + std::unique_ptr _imu_async_spinner; + std::unique_ptr _async_spinner; + + int /* ros::CallbackQueue */ _imu_queue; + + // TODO: Can this be unsigned? + int64_t _frame_count; + + // Node Handles + // TODO: I don't think these exist for ROS2 + int /* ros::NodeHandle */ _nh; + int /* ros::NodeHandle */ _nh_private; + + // Barometer state variables + double _first_depth = 0.0; + + // DVL state + int /* gtsam::PreintegratedCombinedMeasurements */ *_preintegrated_measurements_dvl; + double _prev_dvl_time = 0.0; + double _prev_dvl_local_time = 0.0; + int /* gtsam::Pose3 */ _prev_dvl_local_pose; + int /* gtsam::Rot3 */ _dvl_prev_rot; + int /* gtsam::Vector3 */ _first_dvl_vel; + + // Keyframe states + double _first_keyframe_time; + double _prev_keyframe_time = 0.0; + double _current_keyframe_time = 0.0; + bool _is_new_keyframe = false; // Used to be new_kf_flag + double _keyfram_gap_time; + int /* gtsam::Pose3 */ _latest_keyframe_pose; + int /* gtsam::Pose3 */ _latest_publish_pose; // what does this mean + + // ??? State + int /* gtsam::Pose3 */ _T_w_wd; // what does this mean + + // IMU State + int /* gtsam::Rot3 */ _imu_latest_rot; + int /* gtsam::NavState*/ _latest_imu_prop_state; + int _imu_init_count = 0; // Can this be unsigned? + int _imu_count = 0; // Can this be unsigned? + + std::mutex _mtx; + std::condition_variable _keyframe_cv; + + // Get from config file. + bool _is_using_dvl_v2_factor = true; + bool _is_rot_initialized = false; + + /* TODO: What is save trajectory?? */ + // ros::ServiceServer _save_trajectory_service; + std::vector _keyframe_timestamps; + std::vector _trajectory_timestamps; + std::vector _trajectory_poses; + +private: // methods + /* TODO: What is save trajectory?? */ + // bool save_trajectory(turtlmap::save_trajectory::Request &req, turtlmap::save_trajectory::Response &res); +} +} // namespace localization + +#endif //__POSEGRAPH_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h new file mode 100644 index 00000000..76b1aaee --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h @@ -0,0 +1,84 @@ +#ifndef __PREINTEGRATED_VELOCITY_HELPERS_H__ +#define __PREINTEGRATED_VELOCITY_HELPERS_H__ + +namespace localization { +class PreintegratedVelocityParameters { +public: // Members + int /* gtsam::Matrix3 */ _bias_velocity_covariance; // Continuous time "covariance" describing velocity measurement bias for random walk + int /* gtsam::Matrix3 */ _bias_initial; // Covariance of baias used as an initial estimate + +public: // Methods + PreintegratedVelocityParameters(); + PreintegratedVelocityParameters( + const int /* gtsam::Matrix3 */ &bias_velocity_covariance + const int /* gtsam::Matrix3 */ &bias_initial + ); + PreintegratedVelocityParameters( + const int /*boost::shared_ptr &p*/ + ); + + // TODO: Make as overloaded= and overloaded<< + void print(const std::string &s = "") const; + bool equals(const PreintegratedVelocityParameters &expected, double tolerance = 1e-9) const; + + void set_bias_velocity_covariance(); + void set_bias_initial(); + +private: // Methods + // TODO: What is this? + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar const unsigned int version); + +public: + // TODO: What is this?? + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; + +class PreintegratedVelocityMeasurementsDvlOnly { +public: // Members + int /*Eigen::Matrix */ _preintegrated_measured_covariance; + std::vector _velocity_list; + std::vector _rotations_list; + std::vector _dt_list; + +protected: // Members + friend class DvlOnlyFactor; + +private: // Members + double delta_T_ij; + int /*gtsam::Pose3*/ _accumulated_poses; + int /*gtsam::Point3*/ _accumulated_positions; + int /*gtsam::imuBais::ConstantBias*/ _dvl_bias_for_imu; // Used for the dvl bias + int /*gtsam::Matrix3*/ _delp_delbias_omega; + int /*gtsam::Matrix3*/ _delp_delbias_dvl; // Default initialize as gtsam::Matrix3::Zero(); + +public: // Methods + PreintegratedVelocityMeasurementsDvlOnly(); + ~PreintegratedVelocityMeasurementsDvlOnly(); + + void reset_integration(); + void reset_integration_and_bias(const int /*gtsam::imuBias::ConstantBias*/ &bias) + void integrateMeasurements( + const/*gtsam::Vector3*/ &linear_velocity, + const/*gtsam::Rot3*/ &interpolated_rotation, + double &dt // TODO: Is this an out param? Why is this passed by ref? + ); + + void integrateMeasurementsNoise( + const/*gtsam::Vector3*/ &linear_velocity, + const/*gtsam::Rot3*/ &interpolated_rotation, + double &dt // TODO: Is this an out param? Why is this passed by ref? + const double &fom + ); + + int /*gtsam::Pose3*/ get_accumulated_poses() const; + int /*gtsam::Point3*/ get_accumulated_positions() const; + int /*gtsam::Matrix*/ get_delpij_delbias_omega() const; + int /*gtsam::Matrix*/ get_delpij_delbias_dvl() const; + int /*gtsam::Point3*/ predict(const int /*gtsam::Point3*/ &bias, int /*gtsam::Matrix3*/ &H_bias) const; +}; + +} // namespace localization + +#endif //__PREINTEGRATED_VELOCITY_PARAMETERS_H__ \ No newline at end of file From d51d1d48fc38e554faa0af2052d111f3897c8e35 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 15:12:35 -0400 Subject: [PATCH 06/31] Fixes the package.xml and adds a README.md --- mrobosub_localization_cpp/README.md | 61 +++++++++++++++++++++++++++ mrobosub_localization_cpp/package.xml | 6 +-- 2 files changed, 64 insertions(+), 3 deletions(-) create mode 100644 mrobosub_localization_cpp/README.md diff --git a/mrobosub_localization_cpp/README.md b/mrobosub_localization_cpp/README.md new file mode 100644 index 00000000..77918c75 --- /dev/null +++ b/mrobosub_localization_cpp/README.md @@ -0,0 +1,61 @@ +# mrobosub_localization_cpp + +For [Zayn Baig](https://github.com/imzaynb)'s Winter 2026 Multidisciplinary Design Project, he was tasked with migrating the [Field Robotics Group](https://fieldrobotics.engin.umich.edu/)'s seminal Autonomous Underwater Veheicle (AUV) localization paper [TURTLMap](https://umfieldrobotics.github.io/TURTLMap/) on the Sub. + +This document will serve as a development diary until I complete the entire project, in which case this will turn into more of a mix of rationale and documentation for the localization code. + +One thing I strongly believe is important to the longevity of the codebase is thorough, accurate documentation. I hope my efforts here will be a good first step towards accomplishing this. + +## TODOs +[] I should probably make a Github Project to keep me organized + +[] Figure out how to add downloading GTSAM to the Docker image + +[] Figure out how to add all of the other C++ requirements to the docker image + - could this be added to the CMakeLists.txt file? Not sure how ament works + +[] I have a feeling the state code listed in `PosegraphNode.h` could definitely be split into state classes for `KeyframeState`, `DvlState`, `ImuState`, and `BarometerState`. + +## Development Diary + +### [03/17] Cloning the repository +I realized pretty late that our codebase (as of October 2025) is fully in ROS2 while TURTLMap was written completely in ROS1. As such, this is going to be both a mix of making their code work on our submarine along with migrating their code to ROS2. + +I created this package using +```bash +ros2 pkg create --license Apache-2.0 --build-type ament-cmake mrobosub_localization_cpp +``` + +I then updated the `package.xml` to reflect relavent changes that needed to be made to the file. + +Across the codebase, we use the BSD-2.0 license, but since I am heavily using the [TURTLMap Github repository](https://github.com/umfieldrobotics/TURTLMap) which has this Apache 2.0 license, I thought it best to adhere to the one they use. + +### [03/17-03/18] Porting all of the Header files +I thought a good place to start would be porting over all of the headerfiles under `./include/mrobosub_localization_cpp` into this repository. + +I chose to have member variables for most classes have an `_` prefix instead of an `_` suffix. I the place where I chose not to adhere to this rule is for the structs in which all members are public anyways and there is really no more elaborate code than using the struct to hold related data together. + +I also found it fit to change the variable names and organize the code a little bit. I have a feeling that I will be majorly refactoring some of the code, especially the ROS specific code, later on, but I think gradual change is the best. I think my idea for the time being will be to keep it relatively similar to the source material for now until I get it running in which case I can majorly refactor it later. + +I also had the header files only define all of the functions, not actually implement them. They had a healthy mix of some getters/setters being implemented in the header file, but I prefer consistency, so all function implementations will be in the source files. + +## Citation + +### Acknowledgements +I give a huge thanks to [Dr. Katie Skinner](https://robotics.umich.edu/people/faculty/katie-skinner/) for guiding me with this project. She's such an awesome person! + +### TURTLMap Paper Citation + +```bibtex +@inproceedings{song2024turtlmap, + title={TURTLMap: Real-time Localization and Dense Mapping of Low-texture Underwater Environments with a Low-cost Unmanned Underwater Vehicle}, + author={Song, Jingyu and Bagoren, Onur and Andigani, Razan and Sethuraman, Advaith and Skinner, Katherine A}, + booktitle={2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pages={1191--1198}, + year={2024}, + organization={IEEE} +} +``` + +### TURTLMap Github +You can see the source code provided by the authors for TURTLMap at the [TURTLMap Github repository](https://github.com/umfieldrobotics/TURTLMap). \ No newline at end of file diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml index c9637985..74037063 100644 --- a/mrobosub_localization_cpp/package.xml +++ b/mrobosub_localization_cpp/package.xml @@ -2,9 +2,9 @@ mrobosub_localization_cpp - 0.0.0 - TODO: Package description - ubuntu + 1.0.0 + Factor graph localization using DVL+IMU+Barometer + Michigan Robotic Submarine Apache-2.0 ament_cmake From 01bf4fd35613632a70c4c189d1118fd54d21d7cf Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 20:18:52 +0000 Subject: [PATCH 07/31] Adds GTSAM to Docker and fixes C++ typos --- Dockerfile | 5 ++- mrobosub_localization_cpp/CMakeLists.txt | 41 ++++++++++++++---- mrobosub_localization_cpp/README.md | 43 +++++++++++++++++++ .../mrobosub_localization_cpp/Posegraph.h | 16 +++---- .../mrobosub_localization_cpp/PosegraphNode.h | 11 +++-- .../PreintegratedVelocityHelpers.h | 29 +++++++++---- mrobosub_localization_cpp/package.xml | 1 + 7 files changed, 117 insertions(+), 29 deletions(-) diff --git a/Dockerfile b/Dockerfile index c2e6acb8..eb63631b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -14,6 +14,7 @@ RUN apt-get update && \ less \ ros-humble-ros2-control \ ros-humble-ros2-controllers \ + ros-humble-gtsam \ python3-scipy \ python3-transforms3d \ python3-serial \ @@ -32,7 +33,9 @@ RUN pipx install mypy && \ pipx ensurepath RUN sudo pip3 install --upgrade mypy typing-extensions - + +RUN ln -s '/usr/install/libusb-1.0/libusb.h /usr/install/libusb.h' + # Create workspace structure RUN mkdir -p /home/ubuntu/ros2_ws/src && \ cd /home/ubuntu/ros2_ws && \ diff --git a/mrobosub_localization_cpp/CMakeLists.txt b/mrobosub_localization_cpp/CMakeLists.txt index 3336de18..5ddf602d 100644 --- a/mrobosub_localization_cpp/CMakeLists.txt +++ b/mrobosub_localization_cpp/CMakeLists.txt @@ -7,18 +7,41 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(GTSAM REQUIRED) -add_executable(localization src/localization.cpp) +# Add source files to executable +add_executable(localization + src/localization.cpp +) + +# Ament automates a lot of the CMake process. +# We can use ament to include+link rclcpp in one line +ament_target_dependencies(localization + rclcpp +) + +# Expose the /include directory target_include_directories(localization PUBLIC $ - $) -target_compile_features(localization PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + $ +) + +# Require C99 and C++17 +target_compile_features(localization PUBLIC + c_std_99 cxx_std_17 +) + +# Manually link with gtsam +target_link_libraries(localization + gtsam +) -install(TARGETS localization - DESTINATION lib/${PROJECT_NAME}) +# Copy to the `install/` folder. +install( + TARGETS localization + DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -32,4 +55,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +# If other packages in the workspace need mrobosub_localization_cpp +# ament_export_dependencies(gtsam) ament_package() diff --git a/mrobosub_localization_cpp/README.md b/mrobosub_localization_cpp/README.md index 77918c75..5b6eb25a 100644 --- a/mrobosub_localization_cpp/README.md +++ b/mrobosub_localization_cpp/README.md @@ -39,6 +39,49 @@ I also found it fit to change the variable names and organize the code a little I also had the header files only define all of the functions, not actually implement them. They had a healthy mix of some getters/setters being implemented in the header file, but I prefer consistency, so all function implementations will be in the source files. +### [03/18] Loading the GTSAM library onto this Docker container +The easiest way that I have seen to load GTSAM in for a ROS environment is installing the package via apt. + +```bash +sudo apt install ros-humble-gtsam +``` + +I was considering adding the GTSAM Github as a submodule, but this would drastically increase compilation times which is really not worth it given how frequently we build. + +This line was added to the Dockerfile. + +Additionally, you also need to reflect this dependency in the `package.xml` with the line `gtsam`. + +Also, you need to reflect these changes in the CMakeLists.txt. + +#### 1. Find the package + +We first need to have CMake find the package +``` +find_package(GTSAM required) +``` + +#### 2. Link with the library +``` +target_link_libraries(localization + gtsam +) +``` + +#### 3. (Optional) Expose GTSAM to other modules + +If you need other packages in the workspace need to use this package (`mrobosub_localization_cpp`), they will need GTSAM too. We can expose this to them using ament. +``` +ament_export_dependencies(gtsam) +``` + + +After adding GTSAM, I rebuilt the container, which successfully worked. + +Additionally, I had to add a couple of VSCode extensions for C++ and CMake add better linting and intellisense. With this, I caught some small typos with my code. + + + ## Citation ### Acknowledgements diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h index 5094b519..157291fb 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -70,9 +70,9 @@ class Posegraph { public: // Methods // Ctors + Dtors - AUVPoseGraph(); - AUVPoseGraph(std::string &config_file) - ~AUVPoseGraph(); + Posegraph(); + Posegraph(std::string &config_file); + Posegraph(); // Depth factors void add_depth_factor(); @@ -124,12 +124,12 @@ class Posegraph { // Setters void set_depth_measurement(double W_measurement_z); - void set_accelerometer_measurement(/*gtsam::Vector3*/ B_accelerometer_S); - void set_gyroscope_measurement(/*gtsam::Vector3*/ B_gyroscope_S); - void set_velocity_measurement(/*gtsam::Vector3*/ B_velocity_D); - void set_position_measurement(/*gtsam::Vector3*/ W_position_C); + void set_accelerometer_measurement(int /*gtsam::Vector3*/ B_accelerometer_S); + void set_gyroscope_measurement(int /*gtsam::Vector3*/ B_gyroscope_S); + void set_velocity_measurement(int /*gtsam::Vector3*/ B_velocity_D); + void set_position_measurement(int /*gtsam::Vector3*/ W_position_C); void set_visual_gap_time(double visualGapTime); -} +}; } // namespace localization #endif //__POSEGRAPH_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h index 7bc5b5f7..ac3f3919 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h @@ -1,5 +1,9 @@ -#ifndef __POSEGRAPH_H__ -#define __POSEGRAPH_H__ +#ifndef __POSEGRAPH_NODE_H__ +#define __POSEGRAPH_NODE_H__ + +#include +#include +#include #include "Posegraph.h" #include "Parameters.h" @@ -7,6 +11,7 @@ #include "DvlOnlyFactor.h" #include "BluerovBarometerFactor.h" + namespace localization { class PosegraphNode { public: @@ -88,7 +93,7 @@ class PosegraphNode { private: // methods /* TODO: What is save trajectory?? */ // bool save_trajectory(turtlmap::save_trajectory::Request &req, turtlmap::save_trajectory::Response &res); -} +}; } // namespace localization #endif //__POSEGRAPH_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h index 76b1aaee..bbaa06ef 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h @@ -1,6 +1,12 @@ #ifndef __PREINTEGRATED_VELOCITY_HELPERS_H__ #define __PREINTEGRATED_VELOCITY_HELPERS_H__ +#include +#include + +#include +#include + namespace localization { class PreintegratedVelocityParameters { public: // Members @@ -10,7 +16,7 @@ class PreintegratedVelocityParameters { public: // Methods PreintegratedVelocityParameters(); PreintegratedVelocityParameters( - const int /* gtsam::Matrix3 */ &bias_velocity_covariance + const int /* gtsam::Matrix3 */ &bias_velocity_covariance, const int /* gtsam::Matrix3 */ &bias_initial ); PreintegratedVelocityParameters( @@ -26,12 +32,17 @@ class PreintegratedVelocityParameters { private: // Methods // TODO: What is this? + // Allows boost serialization to access private members. friend class boost::serialization::access; + + // Needed to save data and load data template - void serialize(ARCHIVE &ar const unsigned int version); + void serialize(ARCHIVE &ar, const unsigned int version); public: - // TODO: What is this?? + // GTSAM uses advanced math with Eigen + // This is to ensure that the Matrices and other stuff + // is aligned how GTSAM wants it to be. GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -58,17 +69,17 @@ class PreintegratedVelocityMeasurementsDvlOnly { ~PreintegratedVelocityMeasurementsDvlOnly(); void reset_integration(); - void reset_integration_and_bias(const int /*gtsam::imuBias::ConstantBias*/ &bias) + void reset_integration_and_bias(const int /*gtsam::imuBias::ConstantBias*/ &bias); void integrateMeasurements( - const/*gtsam::Vector3*/ &linear_velocity, - const/*gtsam::Rot3*/ &interpolated_rotation, + const int /*gtsam::Vector3*/ &linear_velocity, + const int /*gtsam::Rot3*/ &interpolated_rotation, double &dt // TODO: Is this an out param? Why is this passed by ref? ); void integrateMeasurementsNoise( - const/*gtsam::Vector3*/ &linear_velocity, - const/*gtsam::Rot3*/ &interpolated_rotation, - double &dt // TODO: Is this an out param? Why is this passed by ref? + const int /*gtsam::Vector3*/ &linear_velocity, + const int /*gtsam::Rot3*/ &interpolated_rotation, + double &dt, // TODO: Is this an out param? Why is this passed by ref? const double &fom ); diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml index 74037063..ce0df833 100644 --- a/mrobosub_localization_cpp/package.xml +++ b/mrobosub_localization_cpp/package.xml @@ -9,6 +9,7 @@ ament_cmake + gtsam ament_lint_auto ament_lint_common From 1681de7f9ee83159764eb6adca5c722ad731056e Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 23:44:30 -0400 Subject: [PATCH 08/31] Adds symbolic link to Dockerfile for libusb.h --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index eb63631b..a017139f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -34,7 +34,7 @@ RUN pipx install mypy && \ RUN sudo pip3 install --upgrade mypy typing-extensions -RUN ln -s '/usr/install/libusb-1.0/libusb.h /usr/install/libusb.h' +# RUN sudo ln -s '/usr/install/libusb-1.0/libusb.h /usr/install/libusb.h' # Create workspace structure RUN mkdir -p /home/ubuntu/ros2_ws/src && \ From 6895e6b7d545d307acab024b9740e4a7f8b7513c Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 23:45:46 -0400 Subject: [PATCH 09/31] Add C++ and CMake extensions to devcontainer --- .devcontainer/devcontainer.json | 4 +++- .vscode/c_cpp_properties.json | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index b89a879c..4c29f9fd 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -8,7 +8,9 @@ "vscode": { "extensions": [ "ms-python.python", - "ms-python.black-formatter" + "ms-python.black-formatter", + "ms-vscode.cpptools", + "ms-vscode.cmake-tools" ] } } diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 27a611bb..a742218e 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -5,7 +5,8 @@ "includePath": [ "${workspaceFolder}/**", "/opt/ros/humble/include/**", - "${workspaceFolder}/../install/**/include/**" + "${workspaceFolder}/../install/**/include/**", + "/usr/include/eigen3" ], "defines": [], "compilerPath": "/usr/bin/gcc", From 156a945a650765e0a421d5b0a3771f19111c70e7 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 23:46:08 -0400 Subject: [PATCH 10/31] Add Eigen and Boost to CMakeLists.txt and dependencies --- mrobosub_localization_cpp/CMakeLists.txt | 11 +++++++++++ mrobosub_localization_cpp/package.xml | 3 +++ 2 files changed, 14 insertions(+) diff --git a/mrobosub_localization_cpp/CMakeLists.txt b/mrobosub_localization_cpp/CMakeLists.txt index 5ddf602d..005878b1 100644 --- a/mrobosub_localization_cpp/CMakeLists.txt +++ b/mrobosub_localization_cpp/CMakeLists.txt @@ -8,7 +8,11 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(Eigen3 REQUIRED) find_package(GTSAM REQUIRED) +find_package(BOOST REQUIRED + COMPONENTS thread +) # Add source files to executable add_executable(localization @@ -21,6 +25,10 @@ ament_target_dependencies(localization rclcpp ) +target_include_directories(localization PRIVATE + ${EIGEN3_INCLUDE_DIRS} +) + # Expose the /include directory target_include_directories(localization PUBLIC $ @@ -34,9 +42,12 @@ target_compile_features(localization PUBLIC # Manually link with gtsam target_link_libraries(localization + Eigen3::Eigen gtsam + ${Boost_LIBRARIES} ) + # Copy to the `install/` folder. install( TARGETS localization diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml index ce0df833..168bb89d 100644 --- a/mrobosub_localization_cpp/package.xml +++ b/mrobosub_localization_cpp/package.xml @@ -9,7 +9,10 @@ ament_cmake + eigen gtsam + gtsam + libeigen3-dev ament_lint_auto ament_lint_common From 8391206526989bb23eff212936b6a335fdd5c637 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 23:47:42 -0400 Subject: [PATCH 11/31] Fix gtsam includes after downloading gtsam --- mrobosub_localization_cpp/README.md | 22 +++- .../BluerovBarometerFactor.h | 17 ++- .../mrobosub_localization_cpp/DvlOnlyFactor.h | 27 +++-- .../mrobosub_localization_cpp/Parameters.h | 27 +++-- .../mrobosub_localization_cpp/Posegraph.h | 112 ++++++++++-------- .../mrobosub_localization_cpp/PosegraphNode.h | 37 +++--- .../PreintegratedVelocityHelpers.h | 64 +++++----- 7 files changed, 191 insertions(+), 115 deletions(-) diff --git a/mrobosub_localization_cpp/README.md b/mrobosub_localization_cpp/README.md index 5b6eb25a..c37c674c 100644 --- a/mrobosub_localization_cpp/README.md +++ b/mrobosub_localization_cpp/README.md @@ -75,12 +75,32 @@ If you need other packages in the workspace need to use this package (`mrobosub_ ament_export_dependencies(gtsam) ``` - After adding GTSAM, I rebuilt the container, which successfully worked. Additionally, I had to add a couple of VSCode extensions for C++ and CMake add better linting and intellisense. With this, I caught some small typos with my code. +### [03/18] Fixing the variables to use the GTSAM library. + +Before, I was not using the `gtsam::` variables because I had not included `gtsam` into the project until after I finished all of the header files. I wanted to get a good bit of coding done, just as a productivity boost, before I get bogged down with downloading and debugging install GTSAM. + +After downloading the library, as I document above, I fully converted all of the commented out gtsam objects into the actual (uncommented out) versions. +In the existing GTSAM code, there is a mix of smart pointers (e.g. `std::shared_ptr` or `std::unique_ptr`) between both the standard library `std` and the `boost` library. However, GTSAM heavily relies on the `boost` library. As such, for consistency sake, I will consistently use `boost`. + +In order to do so, I have to add `Boost` to the CMakeLists.txt in 2 ways: + +#### 1. Add the package +``` +find_package(Boost REQUIRED COMPONENTS thread) +``` + +#### 2. Link the libraries +``` +target_link_libraries(localization PRIVATE +${BOOST_LIBRARIES} +... +) +``` ## Citation diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h index cf883c4f..81ce7f5f 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -1,16 +1,27 @@ #ifndef __BLUEROV_BAROMETER_FACTOR_H__ #define __BLUEROV_BAROMETER_FACTOR_H__ +#include // gtsam::Vector +#include // gtsam::Matrix +#include // gtsam::Pose3 +#include // gtsam::Key +#include // gtsam::NoiseModelFactor1 +#include // gtsam::SharedNoiseModel + namespace localization { -class BluerovBarometerFactor /*: public NoiseModelFactor1 */ { +class BluerovBarometerFactor : public NoiseModelFactor1 { private: double _measured; public: BluerovBarometerFactor(); virtual ~BluerovBarometerFactor(); - BluerovBarometerFactor(int /*gtsam::Key*/ key, double measured, const int /*gtsam::SharedNoiseModle*/ &model); - int /*gtsam::Vector*/ evaluate_error(const int /*gtsam::Pose3*/ &pose, int /*boost::optional*/ H) const; + BluerovBarometerFactor(int gtsam::Key key, double measured, const int gtsam::SharedNoiseModel &model); + + int gtsam::Vector evaluate_error( + const int gtsam::Pose3 &pose, + int boost::optional H + ) const; }; } // namespace localization diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h index 39ad3a2f..3baf1d30 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h @@ -3,20 +3,29 @@ #include "PreintegratedVelocityHelpers.h" -class DvlOnlyFactor /* : public gtsam::NoiseModelFactorN*/ { +#include // Vector +#include +#include // Pose3 +#include // Key +#include // imuBias::ConstantBias +#include // NoiseModelFactorN + +#include + +class DvlOnlyFactor : public gtsam::NoiseModelFactorN { public: // Methods DvlOnlyFactor(); - DVLOnlyFactor(int /*gtsam::Key*/ pose_i, int /*gtsam::Key*/ pose_j, int /*gtsam::Key*/ vbias_i, + DVLOnlyFactor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key vbias_i, const PreintegratedVelocityMeasurementsDvlOnly &pvm); virtual ~DvlOnlyFactor(); - int /*gtsam::Vector*/ evaluate_error( - const int /*gtsam::Pose3*/ &pose_i, - const int /*gtsam::Pose3*/ &pose_j, - const int /*gtsam::imuBias::ConstantBias*/ &vbias_i, - int /*boost::optional*/ H1, - int /*boost::optional*/ H2, - int /*boost::optional*/ H3 + gtsam::Vector evaluate_error( + const gtsam::Pose3 &pose_i, + const gtsam::Pose3 &pose_j, + const gtsam::imuBias::ConstantBias &vbias_i, + boost::optional H1, + boost::optional H2, + boost::optional H3 ) const; private: // Members diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h index 6befa0d0..bf688d41 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h @@ -1,6 +1,11 @@ #ifndef __PARAMETERS_H__ #define __PARAMETERS_H__ +#include + +#include +#include + namespace localization { struct SensorList { bool is_dvl_used; @@ -35,11 +40,11 @@ struct SensorTopics { }; struct Extrinsics { - int /*Eigen::Matrix4d */ T_SD; // dvl to imu - int /*Eigen::Matrix4d */ T_SSo; // sonar to imu - int /*Eigen::Matrix4d */ T_BS; // imu to body - int /*Eigen::Matrix4d */ T_SBa; // barometer to imu - int /*Eigen::Matrix4d */ T_W_WD; // world to dvl world + Eigen::Matrix4d T_SD; // dvl to imu + Eigen::Matrix4d T_SSo; // sonar to imu + Eigen::Matrix4d T_BS; // imu to body + Eigen::Matrix4d T_SBa; // barometer to imu + Eigen::Matrix4d T_W_WD; // world to dvl world }; struct OptimizationParameters { @@ -60,12 +65,12 @@ class Parameters { Parameters(); ~Parameters(); - ImuParameters _imu_params; - Extrinsics _extrinsics; - SensorList _sensor_list; - SensorTopics _sensor_topics; - OptimizationParameters _optimization_params; - ImuPreintegrationParams _imu_preintegration_params; + ImuParameters _imu_params; + Extrinsics _extrinsics; + SensorList _sensor_list; + SensorTopics _sensor_topics; + OptimizationParameters _optimization_params; + ImuPreintegrationParameters _imu_preintegration_params; std::vector _rosbag_topics; diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h index 157291fb..f120b726 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -1,5 +1,22 @@ #ifndef __POSEGRAPH_H__ #define __POSEGRAPH_H__ + +#include "PreintegratedVelocityHelpers.h" +#include "Parameters.h" + +#include // Matrix44 +#include // Vector +#include // Pose3 +#include // Pose3 +#include // NavState +#include // PreintegratedCombinedMeasurements +#include // imuBias::ConstantFactor +#include // ISAM2Params +#include // BatchFixedLabSmoother +#include // FixedLagSmoother::KeyTimestampMap +#include // NonlinearFactorGraph +#include // Values + #include #include #include @@ -10,49 +27,48 @@ class Posegraph { int _index; // Smoother - int /* gtsam::ISAM2Params */ _smoother_parameters; + gtsam::ISAM2Params _smoother_parameters; double _smoother_lag = 6.0; // TODO - int /* gtsam::BatchFixedLagSmoother */ _smoother_ISAM2; - int /* gtsam::FixedLagSmoother::KeyTimestampMap */ _smoother_timestamps; + gtsam::BatchFixedLagSmoother _smoother_ISAM2; + gtsam::FixedLagSmoother::KeyTimestampMap _smoother_timestamps; // GTSAM - int /*gtsam::NonlinearFactorGraph*/ *_graph; - int /*gtsam::Values*/ *_initial; - int /*gtsam::Values*/ *_result; - int /*gtsam::PreintegratedCombinedMeasurements*/ *_preintegrated_measurements; // used to be pim - // BlueRovPreintegratedVelocityMeasurements*/ *_pvm; - int /*PreintegratedVelocityMeasurementsDvlOnly*/ *_preintegrated_velocity_measurements; // used to be pvm - int /*gtsam::PreintegratedCombinedMeasurements::Params*/ *_preintergrated_measurement_params; // used to be pim_params - int /*PoseGraphParameters*/ *_pose_graph_params; // use to be params + gtsam::NonlinearFactorGraph *_graph; + gtsam::Values *_initial; + gtsam::Values *_result; + gtsam::PreintegratedCombinedMeasurements *_preintegrated_measurements; // used to be pim + PreintegratedVelocityMeasurementsDvlOnly *_preintegrated_velocity_measurements; // used to be pvm + gtsam::PreintegratedCombinedMeasurements::Params *_preintergrated_measurement_params; // used to be pim_params + Parameters *_pose_graph_params; // use to be params // Prior imuBias - int /*gtsam::imuBias::ConstantBias*/ _prior_imu_bias; - int /*gtsam::imuBias::ConstantBias*/ _prior_dvl_bias; // TODO: = gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0)); + gtsam::imuBias::ConstantBias _prior_imu_bias; + gtsam::imuBias::ConstantBias _prior_dvl_bias; // TODO: = gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0)); // Transforms - int /*gtsam::Matrix44*/ _T_SD; // sensor (IMU) to DVL affine transform - int /*gtsam::Matrix44*/ _T_SB; // sensor (IMU)to robot center affine transform - int /*gtsam::Matrix44*/ _T_W_WD; // world to DVL world transform + gtsam::Matrix44 _T_SD; // sensor (IMU) to DVL affine transform + gtsam::Matrix44 _T_SB; // sensor (IMU)to robot center affine transform + gtsam::Matrix44 _T_W_WD; // world to DVL world transform // Previous - int /*gtsam::Pose3*/ _prev_pose; - int /*gtsam::Vector3*/ _prev_vel; - int /*gtsam::NavState*/ _prev_state; + gtsam::Pose3 _prev_pose; + gtsam::Vector3 _prev_vel; + gtsam::NavState _prev_state; double _prev_keyframe_time; // OLD TODO: used for saving the previous keyframe time // Current - int /*gtsam::Pose3*/ _current_pose; - int /*gtsam::Vector3*/ _current_vel; - int /*gtsam::NavState*/ _current_state; - int /*gtsam::imuBias::ConstantBias*/ _current_imu_bias; + gtsam::Pose3 _current_pose; + gtsam::Vector3 _current_vel; + gtsam::NavState _current_state; + gtsam::imuBias::ConstantBias _current_imu_bias; double _current_time; - std::vector _current_dvl_vels; - std::vector _current_dvl_poses; - std::vector _current_dvl_rotations; + std::vector _current_dvl_vels; + std::vector _current_dvl_poses; + std::vector _current_dvl_rotations; std::vector _current_dvl_foms; // TODO: What does FOM mean? - std::vector _imu_rot_list; - int /*gtsam::Rot3*/ _imu_prev_rot; + std::vector _imu_rot_list; + gtsam::Rot3 _imu_prev_rot; std::vector _current_dvl_timestamps; std::vector _current_dvl_local_timestamps; @@ -63,10 +79,10 @@ class Posegraph { double _visual_gap_time; double _W_measurement_z; - /* gtsam::Vector3 */ double _B_accelerometer_S; - /* gtsam::Vector3 */ double _B_gyroscope_S; - /* gtsam::Vector3 */ double _B_velocity_D; - /* gtsam::Vector3 */ double _B_position_C; + gtsam::Vector3 _B_accelerometer_S; + gtsam::Vector3 _B_gyroscope_S; + gtsam::Vector3 _B_velocity_D; + gtsam::Vector3 _B_position_C; public: // Methods // Ctors + Dtors @@ -89,45 +105,45 @@ class Posegraph { void add_dvl_odometry_factor(double noise); // DVL Suppliers - void add_dvl_velocity(double time_stamp /* gtsam::Vector3 velocity */); - void add_dvl_pose(double time_stamp /* gtsam::Pose3 pose*/); - void add_dvl_rotation(double time_stamp /* gtsam::Rot3 rotation */); + void add_dvl_velocity(double time_stamp, gtsam::Vector3 velocity); + void add_dvl_pose(double time_stamp, gtsam::Pose3 pose); + void add_dvl_rotation(double time_stamp, gtsam::Rot3 rotation); // Other factors - void add_visual_constraint_factor(/*gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx*/); + void add_visual_constraint_factor(gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx); void add_sonar_factor(); - void add_prior_factor(/*gtsam::Pose3 initial_pose, gtsam::Vector initial_vec, double pose_noise*/); + void add_prior_factor(gtsam::Pose3 initial_pose, gtsam::Vector initial_vec, double pose_noise); // Creating transforms void define_transforms(); // Adding estimates void add_simple_estimate(double dt, double noise); - void add_initial_estimate(/*gtsam::Pose3 initial_pose, gtsam::Vector3 intitial_vel*/); + void add_initial_estimate(gtsam::Pose3 initial_pose, gtsam::Vector3 intitial_vel); void add_visual_estimate(const std::vector vertex); // Working with the pose graph void initialize_pose_graph(); - void initialize_pose_graph_from_imu(/*gtsam::Rot3 initial_rotation*/); + void initialize_pose_graph_from_imu(gtsam::Rot3 initial_rotation); void optimize_pose_graph(); void optimize_pose_graph_smoother(); void add_edges_to_graph(const std::vector> &edges); // Getters double get_depth_measurement(); - void /* gtsam::Vector3 */ get_accelerometer_measurement(); - void /* gtsam::Vector3 */ get_gyroscope_measurement(); - void /* gtsam::Vector3 */ get_velocity_measurement(); - void /* gtsam::Vector3 */ get_position_measurement(); + gtsam::Vector3 get_accelerometer_measurement(); + gtsam::Vector3 get_gyroscope_measurement(); + gtsam::Vector3 get_velocity_measurement(); + gtsam::Vector3 get_position_measurement(); double get_visual_gap_time(); - void /* gtsam::Rot3 */ find_current_pose_for_dvl_vel(double time_stamp); + gtsam::Rot3 find_current_pose_for_dvl_vel(double time_stamp); // Setters void set_depth_measurement(double W_measurement_z); - void set_accelerometer_measurement(int /*gtsam::Vector3*/ B_accelerometer_S); - void set_gyroscope_measurement(int /*gtsam::Vector3*/ B_gyroscope_S); - void set_velocity_measurement(int /*gtsam::Vector3*/ B_velocity_D); - void set_position_measurement(int /*gtsam::Vector3*/ W_position_C); + void set_accelerometer_measurement(gtsam::Vector3 B_accelerometer_S); + void set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S); + void set_velocity_measurement(gtsam::Vector3 B_velocity_D); + void set_position_measurement(gtsam::Vector3 W_position_C); void set_visual_gap_time(double visualGapTime); }; } // namespace localization diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h index ac3f3919..41ed9fec 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h @@ -1,16 +1,21 @@ #ifndef __POSEGRAPH_NODE_H__ #define __POSEGRAPH_NODE_H__ -#include -#include -#include - #include "Posegraph.h" #include "Parameters.h" #include "PreintegratedVelocityHelpers.h" #include "DvlOnlyFactor.h" #include "BluerovBarometerFactor.h" +#include // Vector +#include // Pose3 +#include // Rot3 +#include // PreintegratedCombinedMeasurements +#include // NavState + +#include // boost::mutex +#include // boost::condition_variable +#include // boost::shared_ptr namespace localization { class PosegraphNode { @@ -52,12 +57,12 @@ class PosegraphNode { double _first_depth = 0.0; // DVL state - int /* gtsam::PreintegratedCombinedMeasurements */ *_preintegrated_measurements_dvl; + gtsam::PreintegratedCombinedMeasurements *_preintegrated_measurements_dvl; double _prev_dvl_time = 0.0; double _prev_dvl_local_time = 0.0; - int /* gtsam::Pose3 */ _prev_dvl_local_pose; - int /* gtsam::Rot3 */ _dvl_prev_rot; - int /* gtsam::Vector3 */ _first_dvl_vel; + gtsam::Pose3 _prev_dvl_local_pose; + gtsam::Rot3 _dvl_prev_rot; + gtsam::Vector3 _first_dvl_vel; // Keyframe states double _first_keyframe_time; @@ -65,20 +70,20 @@ class PosegraphNode { double _current_keyframe_time = 0.0; bool _is_new_keyframe = false; // Used to be new_kf_flag double _keyfram_gap_time; - int /* gtsam::Pose3 */ _latest_keyframe_pose; - int /* gtsam::Pose3 */ _latest_publish_pose; // what does this mean + gtsam::Pose3 _latest_keyframe_pose; + gtsam::Pose3 _latest_publish_pose; // what does this mean // ??? State - int /* gtsam::Pose3 */ _T_w_wd; // what does this mean + gtsam::Pose3 _T_w_wd; // what does this mean // IMU State - int /* gtsam::Rot3 */ _imu_latest_rot; - int /* gtsam::NavState*/ _latest_imu_prop_state; + gtsam::Rot3 _imu_latest_rot; + gtsam::NavState _latest_imu_prop_state; int _imu_init_count = 0; // Can this be unsigned? int _imu_count = 0; // Can this be unsigned? - std::mutex _mtx; - std::condition_variable _keyframe_cv; + boost::mutex _mtx; + boost::condition_variable _keyframe_cv; // Get from config file. bool _is_using_dvl_v2_factor = true; @@ -88,7 +93,7 @@ class PosegraphNode { // ros::ServiceServer _save_trajectory_service; std::vector _keyframe_timestamps; std::vector _trajectory_timestamps; - std::vector _trajectory_poses; + std::vector _trajectory_poses; private: // methods /* TODO: What is save trajectory?? */ diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h index bbaa06ef..91808ec8 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h @@ -1,26 +1,36 @@ #ifndef __PREINTEGRATED_VELOCITY_HELPERS_H__ #define __PREINTEGRATED_VELOCITY_HELPERS_H__ +#include // GTSAM_MAKE_ALIGNED_OPERATOR_NEW +#include // gtsam::Matrix3 +#include // gtsam::Vector +#include // gtsam::Rot3 +#include // gtsam::Pose3 +#include // gtsam::Point3 +#include // gtsam::imuBias::ConstantBias + +#include // Eigen::Matrix + +#include // boost::serialize::access, serialize +#include // boost::shared_ptr + #include #include -#include -#include - namespace localization { class PreintegratedVelocityParameters { public: // Members - int /* gtsam::Matrix3 */ _bias_velocity_covariance; // Continuous time "covariance" describing velocity measurement bias for random walk - int /* gtsam::Matrix3 */ _bias_initial; // Covariance of baias used as an initial estimate + gtsam::Matrix3 _bias_velocity_covariance; // Continuous time "covariance" describing velocity measurement bias for random walk + gtsam::Matrix3 _bias_initial; // Covariance of bias used as an initial estimate public: // Methods PreintegratedVelocityParameters(); PreintegratedVelocityParameters( - const int /* gtsam::Matrix3 */ &bias_velocity_covariance, - const int /* gtsam::Matrix3 */ &bias_initial + const gtsam::Matrix3 &bias_velocity_covariance, + const gtsam::Matrix3 &bias_initial ); PreintegratedVelocityParameters( - const int /*boost::shared_ptr &p*/ + const boost::shared_ptr &p ); // TODO: Make as overloaded= and overloaded<< @@ -31,7 +41,6 @@ class PreintegratedVelocityParameters { void set_bias_initial(); private: // Methods - // TODO: What is this? // Allows boost serialization to access private members. friend class boost::serialization::access; @@ -48,9 +57,9 @@ class PreintegratedVelocityParameters { class PreintegratedVelocityMeasurementsDvlOnly { public: // Members - int /*Eigen::Matrix */ _preintegrated_measured_covariance; - std::vector _velocity_list; - std::vector _rotations_list; + Eigen::Matrix _preintegrated_measured_covariance; + std::vector _velocity_list; + std::vector _rotations_list; std::vector _dt_list; protected: // Members @@ -58,36 +67,37 @@ class PreintegratedVelocityMeasurementsDvlOnly { private: // Members double delta_T_ij; - int /*gtsam::Pose3*/ _accumulated_poses; - int /*gtsam::Point3*/ _accumulated_positions; - int /*gtsam::imuBais::ConstantBias*/ _dvl_bias_for_imu; // Used for the dvl bias - int /*gtsam::Matrix3*/ _delp_delbias_omega; - int /*gtsam::Matrix3*/ _delp_delbias_dvl; // Default initialize as gtsam::Matrix3::Zero(); + gtsam::Pose3 _accumulated_poses; + gtsam::Point3 _accumulated_positions; + gtsam::imuBias::ConstantBias _dvl_bias_for_imu; // Used for the dvl bias + gtsam::Matrix3 _delp_delbias_omega; + gtsam::Matrix3 _delp_delbias_dvl; // Default initialize as gtsam::Matrix3::Zero(); public: // Methods PreintegratedVelocityMeasurementsDvlOnly(); ~PreintegratedVelocityMeasurementsDvlOnly(); void reset_integration(); - void reset_integration_and_bias(const int /*gtsam::imuBias::ConstantBias*/ &bias); + void reset_integration_and_bias(const gtsam::imuBias::ConstantBias &bias); void integrateMeasurements( - const int /*gtsam::Vector3*/ &linear_velocity, - const int /*gtsam::Rot3*/ &interpolated_rotation, + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, double &dt // TODO: Is this an out param? Why is this passed by ref? ); void integrateMeasurementsNoise( - const int /*gtsam::Vector3*/ &linear_velocity, - const int /*gtsam::Rot3*/ &interpolated_rotation, + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, double &dt, // TODO: Is this an out param? Why is this passed by ref? const double &fom ); - int /*gtsam::Pose3*/ get_accumulated_poses() const; - int /*gtsam::Point3*/ get_accumulated_positions() const; - int /*gtsam::Matrix*/ get_delpij_delbias_omega() const; - int /*gtsam::Matrix*/ get_delpij_delbias_dvl() const; - int /*gtsam::Point3*/ predict(const int /*gtsam::Point3*/ &bias, int /*gtsam::Matrix3*/ &H_bias) const; + gtsam::Pose3 get_accumulated_poses() const; + gtsam::Point3 get_accumulated_positions() const; + gtsam::Matrix get_delpij_delbias_omega() const; + gtsam::Matrix get_delpij_delbias_dvl() const; + + gtsam::Point3 predict(const gtsam::Point3 &bias, int gtsam::Matrix3 &H_bias) const; }; } // namespace localization From 3da7853b6eb9522b9fd2454cce8b3081040c12c3 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 18 Mar 2026 23:53:10 -0400 Subject: [PATCH 12/31] Fixed some typos in BluerovBarometerFactor --- .../mrobosub_localization_cpp/BluerovBarometerFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h index 81ce7f5f..0ffe44b8 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -16,11 +16,11 @@ class BluerovBarometerFactor : public NoiseModelFactor1 { public: BluerovBarometerFactor(); virtual ~BluerovBarometerFactor(); - BluerovBarometerFactor(int gtsam::Key key, double measured, const int gtsam::SharedNoiseModel &model); + BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model); int gtsam::Vector evaluate_error( - const int gtsam::Pose3 &pose, - int boost::optional H + const gtsam::Pose3 &pose, + boost::optional H ) const; }; } // namespace localization From cb60e04ea583bced7032ccc5c7359e50077e26b3 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Thu, 19 Mar 2026 00:09:38 -0400 Subject: [PATCH 13/31] Fixed more typos --- .../mrobosub_localization_cpp/BluerovBarometerFactor.h | 4 ++-- .../include/mrobosub_localization_cpp/DvlOnlyFactor.h | 6 ++++-- .../include/mrobosub_localization_cpp/Parameters.h | 6 ++---- .../include/mrobosub_localization_cpp/Posegraph.h | 6 +++--- .../PreintegratedVelocityHelpers.h | 2 +- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h index 0ffe44b8..1e83f72a 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -9,7 +9,7 @@ #include // gtsam::SharedNoiseModel namespace localization { -class BluerovBarometerFactor : public NoiseModelFactor1 { +class BluerovBarometerFactor : public gtsam::NoiseModelFactor1 { private: double _measured; @@ -18,7 +18,7 @@ class BluerovBarometerFactor : public NoiseModelFactor1 { virtual ~BluerovBarometerFactor(); BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model); - int gtsam::Vector evaluate_error( + gtsam::Vector evaluate_error( const gtsam::Pose3 &pose, boost::optional H ) const; diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h index 3baf1d30..83483c60 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h @@ -4,7 +4,7 @@ #include "PreintegratedVelocityHelpers.h" #include // Vector -#include +#include // Matrix #include // Pose3 #include // Key #include // imuBias::ConstantBias @@ -12,10 +12,11 @@ #include +namespace localization { class DvlOnlyFactor : public gtsam::NoiseModelFactorN { public: // Methods DvlOnlyFactor(); - DVLOnlyFactor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key vbias_i, + DvlOnlyFactor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key vbias_i, const PreintegratedVelocityMeasurementsDvlOnly &pvm); virtual ~DvlOnlyFactor(); @@ -31,5 +32,6 @@ class DvlOnlyFactor : public gtsam::NoiseModelFactorN +#include // Matrix4d #include #include @@ -84,9 +84,7 @@ class Parameters { double _keyframe_gap_time; double _dvl_fom_threshold; bool _using_pseudo_dvl; -} - - +}; } // namespace localization #endif //__PARAMETERS_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h index f120b726..fae289fb 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -7,13 +7,13 @@ #include // Matrix44 #include // Vector #include // Pose3 -#include // Pose3 +#include // Rot3 #include // NavState #include // PreintegratedCombinedMeasurements #include // imuBias::ConstantFactor #include // ISAM2Params -#include // BatchFixedLabSmoother -#include // FixedLagSmoother::KeyTimestampMap +#include // BatchFixedLabSmoother +#include // FixedLagSmoother::KeyTimestampMap #include // NonlinearFactorGraph #include // Values diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h index 91808ec8..1855dc0e 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h @@ -97,7 +97,7 @@ class PreintegratedVelocityMeasurementsDvlOnly { gtsam::Matrix get_delpij_delbias_omega() const; gtsam::Matrix get_delpij_delbias_dvl() const; - gtsam::Point3 predict(const gtsam::Point3 &bias, int gtsam::Matrix3 &H_bias) const; + gtsam::Point3 predict(const gtsam::Point3 &bias, gtsam::Matrix3 &H_bias) const; }; } // namespace localization From 32eff1e9ff6a7ea6ed2759c11c077cd97b22324d Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Thu, 19 Mar 2026 00:10:02 -0400 Subject: [PATCH 14/31] Completely implements BluerovBarometerFactor.cpp --- .../src/BluerovBarometerFactor.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp diff --git a/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp new file mode 100644 index 00000000..f257c470 --- /dev/null +++ b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp @@ -0,0 +1,27 @@ +#include "BluerovBarometerFactor.h" + +namespace localization { +BluerovBarometerFactor::BluerovBarometerFactor() {} + +BluerovBarometerFactor::BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model) + : NoiseModelFactor1(model, key), _measured(measured) {} + +BluerovBarometerFactor::~BluerovBarometerFactor() {} + +gtsam::Vector BluerovBarometerFactor::evaluate_error( + const gtsam::Pose3 &pose, + boost::optional H +) const { + gtsam::Matrix tH; + gtsam::Vector ret = + (gtsam::Vector(1) << (pose.translation(tH).z() - _measured)).finished(); + + if (H) { + *H = tH.block<1, 6>(2, 0); + } + + double error = (pose.z() - _measured); + + return (gtsam::Vector() << error).finished(); +} +} // namespace localization \ No newline at end of file From 5100c9389751219c80af6b33bab32667a7276a24 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 24 Mar 2026 19:59:50 +0000 Subject: [PATCH 15/31] Adds include directories for the C++ plugin --- .vscode/c_cpp_properties.json | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index a742218e..51d8508a 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -6,7 +6,9 @@ "${workspaceFolder}/**", "/opt/ros/humble/include/**", "${workspaceFolder}/../install/**/include/**", - "/usr/include/eigen3" + "/usr/include/eigen3", + "${workspaceFolder}/mrobosub_localization_cpp/include/mrobosub_localization_cpp", + "/opt/ros/humble/include" ], "defines": [], "compilerPath": "/usr/bin/gcc", From e1ddcbfefac94d9a63c8d869ef785ec62c1cefa5 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 24 Mar 2026 20:06:53 +0000 Subject: [PATCH 16/31] MIgrated the Dvl factor code --- .../PreintegratedVelocityHelpers.h | 9 +- .../src/DvlOnlyFactor.cpp | 67 +++++++++ .../src/PreintegratedVelocityHelpers.cpp | 130 ++++++++++++++++++ 3 files changed, 202 insertions(+), 4 deletions(-) create mode 100644 mrobosub_localization_cpp/src/DvlOnlyFactor.cpp create mode 100644 mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h index 1855dc0e..6656a026 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h @@ -37,8 +37,8 @@ class PreintegratedVelocityParameters { void print(const std::string &s = "") const; bool equals(const PreintegratedVelocityParameters &expected, double tolerance = 1e-9) const; - void set_bias_velocity_covariance(); - void set_bias_initial(); + void set_bias_velocity_covariance(const gtsam::Matrix3 &covariance); + void set_bias_initial(const gtsam::Matrix3 &covariance); private: // Methods // Allows boost serialization to access private members. @@ -79,13 +79,13 @@ class PreintegratedVelocityMeasurementsDvlOnly { void reset_integration(); void reset_integration_and_bias(const gtsam::imuBias::ConstantBias &bias); - void integrateMeasurements( + void integrate_measurements( const gtsam::Vector3 &linear_velocity, const gtsam::Rot3 &interpolated_rotation, double &dt // TODO: Is this an out param? Why is this passed by ref? ); - void integrateMeasurementsNoise( + void integrate_measurements_noise( const gtsam::Vector3 &linear_velocity, const gtsam::Rot3 &interpolated_rotation, double &dt, // TODO: Is this an out param? Why is this passed by ref? @@ -96,6 +96,7 @@ class PreintegratedVelocityMeasurementsDvlOnly { gtsam::Point3 get_accumulated_positions() const; gtsam::Matrix get_delpij_delbias_omega() const; gtsam::Matrix get_delpij_delbias_dvl() const; + gtsam::Matrix get_preintegrated_measured_covariance() const; gtsam::Point3 predict(const gtsam::Point3 &bias, gtsam::Matrix3 &H_bias) const; }; diff --git a/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp b/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp new file mode 100644 index 00000000..cef4e1c0 --- /dev/null +++ b/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp @@ -0,0 +1,67 @@ +#include "DvlOnlyFactor.h" + +namespace localization { +DvlOnlyFactor::DvlOnlyFactor() {} + +DvlOnlyFactor::DvlOnlyFactor( + gtsam::Key pose_i, + gtsam::Key pose_j, + gtsam::Key vbias_i, + const PreintegratedVelocityMeasurementsDvlOnly &pvm) + : gtsam::NoiseModelFactorN( + gtsam::noiseModel::Gaussian::Covariance( + _pvm._preintegrated_measured_covariance + ), + pose_i, + pose_j, + vbias_i + ) + , _pvm(pvm) {} + +DvlOnlyFactor::~DvlOnlyFactor() {} + +gtsam::Vector DvlOnlyFactor::evaluate_error( + const gtsam::Pose3 &pose_i, + const gtsam::Pose3 &pose_j, + const gtsam::imuBias::ConstantBias &vbias_i, + boost::optional H1, + boost::optional H2, + boost::optional H3 +) const { + gtsam::Rot3 R_i = pose_i.rotation(); + gtsam::Point3 p_i = pose_i.translation(); + + gtsam::Rot3 R_j = pose_j.rotation(); + gtsam::Point3 p_j = pose_j.translation(); + + gtsam::Matrix3 H_vbias; + gtsam::Point3 vbias = vbias_i.accelerometer(); + gtsam::Point3 accumulated_translation = _pvm.predict(vbias, H_vbias); + + // Compute the inverse of R_i and save as a Rot3 + gtsam::Vector3 residual = gtsam::Rot3(R_i + .matrix() + .transpose() + ).rotate(p_j - p_i); + residual -= accumulated_translation; + + if (H1) { + H1->resize(3, 6); + H1->block<3, 3>(0, 0) = -gtsam::Matrix3::Identity(); + H1->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); + } + if (H2) { + H2->resize(3, 6); + H2->block<3, 3>(0, 0) = R_i.transpose().matrix() * R_j.matrix(); + H2->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); + } + if (H3) { + H3->resize(3, 6); + H3->block<3, 3>(0, 0) = H_vbias; + H3->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); + } + return residual; +} + + +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp b/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp new file mode 100644 index 00000000..c156f0ee --- /dev/null +++ b/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp @@ -0,0 +1,130 @@ +#include "PreintegratedVelocityHelpers.h" + +namespace localization { + PreintegratedVelocityParameters::PreintegratedVelocityParameters() + : _bias_velocity_covariance(gtsam::Matrix3::Zero()) + , _bias_initial(gtsam::Matrix3::Zero()) {} + + PreintegratedVelocityParameters::PreintegratedVelocityParameters( + const gtsam::Matrix3 &bias_velocity_covariance, + const gtsam::Matrix3 &bias_initial + ) : _bias_velocity_covariance(bias_velocity_covariance) + , _bias_initial(bias_initial) {} + + PreintegratedVelocityParameters::PreintegratedVelocityParameters( + const boost::shared_ptr &p + ) { + _bias_velocity_covariance = p->_bias_velocity_covariance; + _bias_initial = p->_bias_initial; + } + + // TODO: Make as overloaded= and overloaded<< + // void PreintegratedVelocityParameters::print(const std::string &s = "") const; + // bool PreintegratedVelocityParameters::equals(const PreintegratedVelocityParameters &expected, double tolerance = 1e-9) const; + + void PreintegratedVelocityParameters::set_bias_velocity_covariance(const gtsam::Matrix3 &covariance) { + _bias_velocity_covariance = covariance; + } + void PreintegratedVelocityParameters::set_bias_initial(const gtsam::Matrix3 &covariance) { + _bias_initial = covariance; + } + + template + void PreintegratedVelocityParameters::serialize(ARCHIVE &ar, const unsigned int version) { + ar &BOOST_SERIALIZATION_NVP(_bias_velocity_covariance); + ar &BOOST_SERIALIZATION_NVP(_bias_initial); + } + + PreintegratedVelocityMeasurementsDvlOnly::PreintegratedVelocityMeasurementsDvlOnly() + : _preintegrated_measured_covariance(gtsam::Matrix::Zero()) {} + + PreintegratedVelocityMeasurementsDvlOnly::~PreintegratedVelocityMeasurementsDvlOnly() {} + + void PreintegratedVelocityMeasurementsDvlOnly::reset_integration() { + _accumulated_poses = gtsam::Pose3(); + _accumulated_positions = gtsam::Point3(0, 0, 0); + _delp_delbias_dvl = gtsam::Matrix3::Zero(); + _preintegrated_measured_covariance = gtsam::Matrix3::Zero(); + _velocity_list.clear(); + _rotations_list.clear(); + _dt_list.clear(); + } + + void PreintegratedVelocityMeasurementsDvlOnly::reset_integration_and_bias(const gtsam::imuBias::ConstantBias &bias) { + reset_integration(); + _dvl_bias_for_imu = bias; + } + + void PreintegratedVelocityMeasurementsDvlOnly::integrate_measurements( + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, + double &dt // TODO: Is this an out param? Why is this passed by ref? + ) { + if (dt <= 0) { + std::cout << "Warning: dt <= 0\n"; + dt = 0.0001; + } + + _velocity_list.push_back(linear_velocity); + _rotations_list.push_back(interpolated_rotation); + _dt_list.push_back(dt); + } + + void PreintegratedVelocityMeasurementsDvlOnly::integrate_measurements_noise( + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, + double &dt, // TODO: Is this an out param? Why is this passed by ref? + const double &fom + ) { + if (dt <= 0) { + std::cout << "Warning: dt <= 0\n"; + dt = 0.0001; + } + + // TODO: Investigate this. + gtsam::Matrix3 B = interpolated_rotation.matrix() * dt; + gtsam::Matrix3 dvl_covariance = fom * gtsam::I_3x3; + _preintegrated_measured_covariance += B * dvl_covariance * B.transpose(); + + _velocity_list.push_back(linear_velocity); + _rotations_list.push_back(interpolated_rotation); + _dt_list.push_back(dt); + } + + gtsam::Point3 PreintegratedVelocityMeasurementsDvlOnly::predict(const gtsam::Point3 &bias, gtsam::Matrix3 &H_bias) const { + gtsam::Point3 predicted_position; + + // Resets the H_bias + H_bias = gtsam::Matrix3::Zero(); + + // integrate using _velocity_list and _rotations_list + for (int i = 0; i < _velocity_list.size(); ++i) { + gtsam::Vector3 velocity = _velocity_list[i] - bias; + gtsam::Vector3 delta_pos = _rotations_list[i].matrix() * velocity * _dt_list[i]; // Looks like pretty bog-standard integration to me. + predicted_position += delta_pos; + H_bias += _rotations_list[i].matrix() * _dt_list[i]; // What is this bias? + } + + return predicted_position; + } + + gtsam::Pose3 PreintegratedVelocityMeasurementsDvlOnly::get_accumulated_poses() const { + return _accumulated_poses; + } + + gtsam::Point3 PreintegratedVelocityMeasurementsDvlOnly::get_accumulated_positions() const { + return _accumulated_positions; + } + + gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_delpij_delbias_omega() const { + return _delp_delbias_omega; + } + + gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_delpij_delbias_dvl() const { + return _delp_delbias_dvl; + } + + gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_preintegrated_measured_covariance() const { + return _preintegrated_measured_covariance; + } +} // namespace localization \ No newline at end of file From dc8ac38727168c65440da75d358c24b8c856e48d Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 24 Mar 2026 20:07:21 +0000 Subject: [PATCH 17/31] Added ROS parameter support and migrates Parameter classes --- .../mrobosub_localization_cpp/Parameters.h | 79 +++++--- .../params/posegraph.yaml | 113 +++++++++++ mrobosub_localization_cpp/src/Parameters.cpp | 190 ++++++++++++++++++ 3 files changed, 357 insertions(+), 25 deletions(-) create mode 100644 mrobosub_localization_cpp/params/posegraph.yaml create mode 100644 mrobosub_localization_cpp/src/Parameters.cpp diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h index c4f11623..e589bbc0 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h @@ -3,16 +3,30 @@ #include // Matrix4d +#include + #include #include namespace localization { -struct SensorList { +struct SensorUsage { bool is_dvl_used; bool is_imu_used; bool is_barometer_used; bool is_sonar_used; bool are_cams_used; + + SensorUsage(std::shared_ptr node); +}; + +struct SensorTopics { + std::string imu_topic; + std::string dvl_topic; + std::string dvl_local_position_topic; + std::string barometer_topic; + std::string sonar_topic; + + SensorTopics(std::shared_ptr node); }; struct ImuParameters { @@ -27,24 +41,28 @@ struct ImuParameters { double imu_rate; double integration_covariance; double g; - double dvl_bias_prior; double dt_imu; + + ImuParameters(std::shared_ptr); }; -struct SensorTopics { - std::string imu_topic; - std::string dvl_topic; - std::string barometer_topic; - std::string sonar_topic; - std::string dvl_local_position_topic; +struct ImuPreintegrationParameters { + double gap_time; + + ImuPreintegrationParameters(std::shared_ptr node); }; -struct Extrinsics { - Eigen::Matrix4d T_SD; // dvl to imu - Eigen::Matrix4d T_SSo; // sonar to imu - Eigen::Matrix4d T_BS; // imu to body - Eigen::Matrix4d T_SBa; // barometer to imu - Eigen::Matrix4d T_W_WD; // world to dvl world +struct DvlParameters { + double prior_bias; + double fom_threshold; + + DvlParameters(std::shared_ptr node); +}; + +struct BarometerParameters { + double atmospheric_pressure; + + BarometerParameters(std::shared_ptr node); }; struct OptimizationParameters { @@ -54,35 +72,46 @@ struct OptimizationParameters { int max_iterations; double relative_error_tolerance; double absolute_error_tolerance; + + OptimizationParameters(std::shared_ptr node); }; -struct ImuPreintegrationParameters { - double gap_time; +struct Extrinsics { + Eigen::Matrix4d T_SD; // dvl to imu + Eigen::Matrix4d T_SSo; // sonar to imu + Eigen::Matrix4d T_BS; // imu to body + Eigen::Matrix4d T_SBa; // barometer to imu + Eigen::Matrix4d T_W_WD; // world to dvl world + + Extrinsics(std::shared_ptr node); }; class Parameters { public: - Parameters(); + Parameters(std::shared_ptr node); ~Parameters(); - ImuParameters _imu_params; - Extrinsics _extrinsics; - SensorList _sensor_list; + // About the sensors + SensorUsage _sensor_usage; SensorTopics _sensor_topics; - OptimizationParameters _optimization_params; + + // Parameters + ImuParameters _imu_params; ImuPreintegrationParameters _imu_preintegration_params; + DvlParameters _dvl_params; + BarometerParameters _barometer_params; + OptimizationParameters _optimization_params; + Extrinsics _extrinsics; + + // Why is this not parameterized? std::vector _rosbag_topics; // TODO: Can we remove this? bool _using_orbslam; - - // TODO: Parse these into separate parameter classes. int _num_iters; - double _baro_atm_pressure; bool _using_smoother; double _keyframe_gap_time; - double _dvl_fom_threshold; bool _using_pseudo_dvl; }; } // namespace localization diff --git a/mrobosub_localization_cpp/params/posegraph.yaml b/mrobosub_localization_cpp/params/posegraph.yaml new file mode 100644 index 00000000..754b9142 --- /dev/null +++ b/mrobosub_localization_cpp/params/posegraph.yaml @@ -0,0 +1,113 @@ +posegraph: + ros__parameters: + # IMX-5 TODO: Read the datasheet + imu_parameters: + g: 0.0 + acc_max: 0.0 + acc_noise_density: 0.0 # + acc_bias_prior: 0.0 + acc_random_walk: 0.0 # [m/s^2/sqrt(Hz)] + gyro_max: 0.0 + gyro_noise_density: 0.0 # [rad/s/sqrt(Hz)] + gyro_bias_prior: 0.0 #[rad/s] + gyro_random_walk: 0.0 # [rad/s^2/sqrt(Hz)] + integration_covarience: 0.0001 + imu_rate: 250.0 #[Hz?] Verify from the datasheet + T_BS: + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + imu_preintegration_parameters: + gap_time: 1.0 + # What is the sonar? + sonar_parameters: + T_SSo: # What is this parameter? + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + dvl_parameters: + T_SD: # What is this parameter? + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + T_W_WD: # What is this parameter? + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + prior_bias: 0.0 + fom_threshold: 0.01 + barometer_parameters: + T_SBa: # What is this parameter? + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + atmospheric_pressure: 993.3999633789062 + optimization_parameters: + lambda_upper_bound: 1e10 + lambda_lower_bound: 1e-10 + lambda_initial: 1e-5 + max_iterations: 500 + relative_error_tolerance: 1e-4 + absolute_error_tolerance: 1e-4 + sensor_topics: + imu_topic: "/todo" + dvl_topic: "/todo" + dvl_local_position_topic: "/todo" + barometer_topic: "/todo" + sonar_topic: "/unused" + # left_cam_topic: + # right_cam_topic: + sensor_usage: + is_dvl_used: true + is_imu_used: true + is_barometer_used: true + is_sonar_used: false + are_cams_used: false + use_orb_slam: false + num_iters: 1000 + using_smoother: true + kf_gap_time: 1.0 + using_pseudo_dvl: true + +# [3/24]: As of right now, I have no plans on using the cameras. + # left_cam_params: + # T_SC: + # image_dimension: + # distortion_coefficients: + # distortion_type: + # focal_length: + # principal_point: + # right_cam_params: + # T_SC: + # image_dimension: + # distortion_coefficients: + # distortion_type: + # focal_length: + # principal_point: + # camera_parameters: + # camera_rate: 30 + # sigma_absolute_translation: 0.0 + # sigma_absolute_orientation: + # sigma_c_relative_translation: + # sigma_c_relative_orientation: + + + + + + diff --git a/mrobosub_localization_cpp/src/Parameters.cpp b/mrobosub_localization_cpp/src/Parameters.cpp new file mode 100644 index 00000000..93bf2f22 --- /dev/null +++ b/mrobosub_localization_cpp/src/Parameters.cpp @@ -0,0 +1,190 @@ +#include "Parameters.h" + +#include + +namespace localization { + SensorUsage::SensorUsage(std::shared_ptr node) { + node->declare_parameter("sensor_usage.is_dvl_used", true); + is_dvl_used = node->get_parameter("sensor_usage.is_dvl_used").as_bool(); + + node->declare_parameter("sensor_usage.is_imu_used", true); + is_imu_used = node->get_parameter("sensor_usage.is_imu_used").as_bool(); + + node->declare_parameter("sensor_usage.is_barometer_used", true); + is_barometer_used = node->get_parameter("sensor_usage.is_barometer_used").as_bool(); + + node->declare_parameter("sensor_usage.is_sonar_used", false); + is_sonar_used = node->get_parameter("sensor_usage.is_sonar_used").as_bool(); + + node->declare_parameter("sensor_usage.are_cams_used", false); + are_cams_used = node->get_parameter("sensor_usage.are_cams_used").as_bool(); + } + + SensorTopics::SensorTopics(std::shared_ptr node) { + node->declare_parameter("sensor_topics.imu_topic", "/todo"); + imu_topic = node->get_parameter("sensor_topics.imu_topic").as_string(); + + node->declare_parameter("sensor_topics.dvl_topic", "/todo"); + dvl_topic = node->get_parameter("sensor_topics.dvl_topic").as_string(); + + node->declare_parameter("sensor_topics.dvl_local_position_topic", "/todo"); + dvl_local_position_topic = node->get_parameter("sensor_topics.dvl_local_position_topic").as_string(); + + node->declare_parameter("sensor_topics.barometer_topic", "/todo"); + barometer_topic = node->get_parameter("sensor_topics.barometer_topic").as_string(); + + node->declare_parameter("sensor_topics.sonar_topic", "/unused"); + sonar_topic = node->get_parameter("sensor_topics.sonar_topic").as_string(); + } + + ImuParameters::ImuParameters(std::shared_ptr node) { + node->declare_parameter("imu_parameters.g", 0.0); + g = node->get_parameter("imu_parameters.g").as_double(); + + node->declare_parameter("imu_parameters.acc_max", 0.0); + acc_max = node->get_parameter("imu_parameters.acc_max").as_double(); + + node->declare_parameter("imu_parameters.acc_noise_density", 0.0); + acc_noise_density = node->get_parameter("imu_parameters.acc_noise_density").as_double(); + + node->declare_parameter("imu_parameters.acc_bias_prior", 0.0); + acc_bias_prior = node->get_parameter("imu_parameters.acc_bias_prior").as_double(); + + node->declare_parameter("imu_parameters.acc_random_walk", 0.0); + acc_random_walk = node->get_parameter("imu_parameters.acc_random_walk").as_double(); + + node->declare_parameter("imu_parameters.gyro_max", 0.0); + gyro_max = node->get_parameter("imu_parameters.gyro_max").as_double(); + + node->declare_parameter("imu_parameters.gyro_noise_density", 0.0); + gyro_noise_density = node->get_parameter("imu_parameters.gyro_noise_density").as_double(); + + node->declare_parameter("imu_parameters.gyro_bias_prior", 0.0); + gyro_bias_prior = node->get_parameter("imu_parameters.gyro_bias_prior").as_double(); + + node->declare_parameter("imu_parameters.gyro_random_walk", 0.0); + gyro_random_walk = node->get_parameter("imu_parameters.gyro_random_walk").as_double(); + + node->declare_parameter("imu_parameters.integration_covarience", 0.0001); + integration_covariance = node->get_parameter("imu_parameters.integration_covarience").as_double(); + + node->declare_parameter("imu_parameters.imu_rate", 250.0); + imu_rate = node->get_parameter("imu_parameters.imu_rate").as_double(); + + dt_imu = 1 / imu_rate; + } + + ImuPreintegrationParameters::ImuPreintegrationParameters(std::shared_ptr node) { + node->declare_parameter("imu_preintegration_parameters.gap_time", 1.0); + gap_time = node->get_parameter("imu_preintegration_parameters.gap_time").as_double(); + } + + DvlParameters::DvlParameters(std::shared_ptr node) { + node->declare_parameter("dvl_parameters.prior_bias", 0.01); + prior_bias = node->get_parameter("dvl_parameters.prior_bias").as_double(); + + node->declare_parameter("dvl_parameters.fom_threshold", 0.01); + fom_threshold = node->get_parameter("dvl_parameters.fom_threshold").as_double(); + } + + BarometerParameters::BarometerParameters(std::shared_ptr node) { + node->declare_parameter("barometer_parameters.atmospheric_pressure", 993.3999633789062); + atmospheric_pressure = node->get_parameter("barometer_parameters.atmospheric_pressure").as_double(); + } + + OptimizationParameters::OptimizationParameters(std::shared_ptr node){ + node->declare_parameter("optimization_parameters.lambda_upper_bound", 1e10); + lambda_upper_bound = node->get_parameter("optimization_parameters.lambda_upper_bound").as_double(); + + node->declare_parameter("optimization_parameters.lambda_lower_bound", 1e-10); + lambda_lower_bound = node->get_parameter("optimization_parameters.lambda_lower_bound").as_double(); + + node->declare_parameter("optimization_parameters.lambda_initial", 1e-5); + lambda_initial = node->get_parameter("optimization_parameters.lambda_initial").as_double(); + + node->declare_parameter("optimization_parameters.max_iterations", 500); + max_iterations = node->get_parameter("optimization_parameters.max_iterations").as_double(); + + node->declare_parameter("optimization_parameters.relative_error_tolerance", 1e-4); + relative_error_tolerance = node->get_parameter("optimization_parameters.relative_error_tolerance").as_double(); + + node->declare_parameter("optimization_parameters.absolute_error_tolerance", 1e-4); + absolute_error_tolerance = node->get_parameter("optimization_parameters.absolute_error_tolerance").as_double(); + } + + Extrinsics::Extrinsics(std::shared_ptr node) { + node->declare_parameter("imu_parameters.T_BS", std::vector()); + std::vector T_BS_flat = node->get_parameter("imu_parameters.T_BS").as_double_array(); + for(int i = 0; i < 4; ++i) { + for(int j = 0; j < 4; ++j) { + T_BS(i, j) = T_BS_flat[i * 4 + j]; + } + } + + node->declare_parameter("sonar_parameters.T_SSo", std::vector()); + std::vector T_SSo_flat = node->get_parameter("sonar_parameters.T_SSo").as_double_array(); + for(int i = 0; i < 4; ++i) { + for(int j = 0; j < 4; ++j) { + T_SSo(i, j) = T_SSo_flat[i * 4 + j]; + } + } + + node->declare_parameter("dvl_parameters.T_SD", std::vector()); + std::vector T_SD_flat = node->get_parameter("dvl_parameters.T_SD").as_double_array(); + for(int i = 0; i < 4; ++i) { + for(int j = 0; j < 4; ++j) { + T_SD(i, j) = T_SD_flat[i * 4 + j]; + } + } + + node->declare_parameter("dvl_parameters.T_W_WD", std::vector()); + std::vector T_W_WD_flat = node->get_parameter("dvl_parameters.T_W_WD").as_double_array(); + for(int i = 0; i < 4; ++i) { + for(int j = 0; j < 4; ++j) { + T_W_WD(i, j) = T_W_WD_flat[i * 4 + j]; + } + } + + node->declare_parameter("barometer_parameters.T_SBa", std::vector()); + std::vector T_SBa_flat = node->get_parameter("barometer_parameters.T_SBa").as_double_array(); + for(int i = 0; i < 4; ++i) { + for(int j = 0; j < 4; ++j) { + T_SBa(i, j) = T_SBa_flat[i * 4 + j]; + } + } + } + + Parameters::Parameters(std::shared_ptr node) + : _sensor_usage(node) + , _sensor_topics(node) + , _imu_params(node) + , _imu_preintegration_params(node) + , _dvl_params(node) + , _barometer_params(node) + , _optimization_params(node) + , _extrinsics(node) { + + node->declare_parameter("using_orbslam", false); + _using_orbslam = node->get_parameter("using_orbslam").as_bool(); + + node->declare_parameter("num_iters", 1000); + _num_iters = node->get_parameter("num_iters").as_int(); + + node->declare_parameter("using_smoother", true); + _using_smoother = node->get_parameter("using_smoother").as_bool(); + + node->declare_parameter("keyframe_gap_time", 1.0); + _keyframe_gap_time = node->get_parameter("keyframe_gap_time").as_double(); + + node->declare_parameter("using_pseudo_dvl", true); + _using_pseudo_dvl = node->get_parameter("using_pseudo_dvl").as_bool(); + + _rosbag_topics.push_back(_sensor_topics.imu_topic); + _rosbag_topics.push_back(_sensor_topics.dvl_topic); + _rosbag_topics.push_back(_sensor_topics.dvl_local_position_topic); + _rosbag_topics.push_back(_sensor_topics.barometer_topic); + // _rosbag_topics.push_back(_sensor_topics.sonar_topic); + } + + Parameters::~Parameters() {} +} // namespace localization \ No newline at end of file From 8f4555327ea5c0f80abd69f8e580782beecf3541 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 24 Mar 2026 20:08:02 +0000 Subject: [PATCH 18/31] Creates constructors and method to initialize parameters for Posegraph --- mrobosub_localization_cpp/README.md | 34 ++++++ .../mrobosub_localization_cpp/Posegraph.h | 45 ++++--- mrobosub_localization_cpp/src/Posegraph.cpp | 115 ++++++++++++++++++ 3 files changed, 174 insertions(+), 20 deletions(-) create mode 100644 mrobosub_localization_cpp/src/Posegraph.cpp diff --git a/mrobosub_localization_cpp/README.md b/mrobosub_localization_cpp/README.md index c37c674c..3b2e5d36 100644 --- a/mrobosub_localization_cpp/README.md +++ b/mrobosub_localization_cpp/README.md @@ -102,6 +102,40 @@ ${BOOST_LIBRARIES} ) ``` +### [03/18] Migrating the Source Files +Before I get to ironing out the specific differences between ROS1 and ROS2 between the two C++ codebases, I want to migrate over all of the ROS-agnostic source code. + +So far, I have migrated `BluerovBarometerFactor`. + +### [03/24] Continuing to Migrate Source Files +I have worked through migrating `DvlOnlyFactor`, `PreintegratedVelocityHelpers`, and started `Posegraph`. + +As I began to work through `Posegraph`, one big thing I encountered was the loading of parameters. The existing solution used by the TURTLMap authors is to use YAML files loaded in by an external YAML C++ library. ROS2 has pretty robust parameter loading (not sure about ROS1 which may have motivated the authors to use the external YAML package). As such, I worked to convert the existing method of loading parameters to the ROS2 method. + +#### About Parameters +While refactoring the parameter code, I returned back to the ROS2 Parameter documentation. Here are the relevant pages. +- [Parameter Documentation](https://docs.ros.org/en/kilted/p/rclcpp/generated/classrclcpp_1_1Parameter.html) +- [Using Parameters in a C++ Class](https://docs.ros.org/en/kilted/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html) +- [Understanding ROS2 Parameters](https://docs.ros.org/en/foxy/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters/Understanding-ROS2-Parameters.html) +- [Using ROS2 Launch For Large Projects](https://docs.ros.org/en/kilted/Tutorials/Intermediate/Launch/Using-ROS2-Launch-For-Large-Projects.html#loadingparametersfromyamlfile) + +Here is the general format of a ROS2 parameter file: +```yaml +: + ros__parameters: + param_one: + parent_one: + child_one: + # ... +``` + +What I didn't understand until just recently is that in order to access _nested_ parameters, you can use `.` syntax. E.g. to access `child_one`, you would access it via `parent_one.child_one`. + +In order to access a node's parameters, you have to have a pointer to the node in question. For me, I wanted to separate the initial parameter loading logic into its own class (see `Parameter.h` and `Parameter.cpp`). Thus, in order to accomplish this, you need to pass a pointer to the node into the `Parameter` class. + +If you want to access a shared_ptr to `this` you can use `this->shared_from_this`. However, you can't pass a `shared_ptr` of a class into any other class or for any other use-case (as it does not exist!!!) until **AFTER THE CONSTRUCTOR RUNS**. I did not know this! As such, you must have a separate `initialize_parameters` method that you will run to populate the parameters after + + ## Citation ### Acknowledgements diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h index fae289fb..6ccc7628 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -3,6 +3,7 @@ #include "PreintegratedVelocityHelpers.h" #include "Parameters.h" +#include "PosegraphNode.h" #include // Matrix44 #include // Vector @@ -20,6 +21,7 @@ #include #include #include +#include // std::unique_ptr namespace localization { class Posegraph { @@ -33,21 +35,22 @@ class Posegraph { gtsam::FixedLagSmoother::KeyTimestampMap _smoother_timestamps; // GTSAM - gtsam::NonlinearFactorGraph *_graph; - gtsam::Values *_initial; - gtsam::Values *_result; - gtsam::PreintegratedCombinedMeasurements *_preintegrated_measurements; // used to be pim - PreintegratedVelocityMeasurementsDvlOnly *_preintegrated_velocity_measurements; // used to be pvm - gtsam::PreintegratedCombinedMeasurements::Params *_preintergrated_measurement_params; // used to be pim_params - Parameters *_pose_graph_params; // use to be params + // TODO: Use boost::unique_ptr? + boost::shared_ptr _graph; + boost::shared_ptr _initial; + boost::shared_ptr _result; + boost::shared_ptr _preintegrated_measurements; // used to be pim + boost::shared_ptr _preintegrated_velocity_measurements; // used to be pvm + boost::shared_ptr _preintergrated_measurement_params; // used to be pim_params + std::unique_ptr _pose_graph_params; // use to be params // Prior imuBias gtsam::imuBias::ConstantBias _prior_imu_bias; - gtsam::imuBias::ConstantBias _prior_dvl_bias; // TODO: = gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0)); + gtsam::imuBias::ConstantBias _prior_dvl_bias; // Transforms gtsam::Matrix44 _T_SD; // sensor (IMU) to DVL affine transform - gtsam::Matrix44 _T_SB; // sensor (IMU)to robot center affine transform + gtsam::Matrix44 _T_SB; // sensor (IMU) to robot center affine transform gtsam::Matrix44 _T_W_WD; // world to DVL world transform // Previous @@ -82,13 +85,15 @@ class Posegraph { gtsam::Vector3 _B_accelerometer_S; gtsam::Vector3 _B_gyroscope_S; gtsam::Vector3 _B_velocity_D; - gtsam::Vector3 _B_position_C; + gtsam::Vector3 _W_position_C; public: // Methods // Ctors + Dtors Posegraph(); - Posegraph(std::string &config_file); - Posegraph(); + ~Posegraph(); + + // Initialize parameters + void initialize_parameters(std::shared_ptr node); // Depth factors void add_depth_factor(); @@ -130,13 +135,13 @@ class Posegraph { void add_edges_to_graph(const std::vector> &edges); // Getters - double get_depth_measurement(); - gtsam::Vector3 get_accelerometer_measurement(); - gtsam::Vector3 get_gyroscope_measurement(); - gtsam::Vector3 get_velocity_measurement(); - gtsam::Vector3 get_position_measurement(); - double get_visual_gap_time(); - gtsam::Rot3 find_current_pose_for_dvl_vel(double time_stamp); + double get_depth_measurement() const; + gtsam::Vector3 get_accelerometer_measurement() const; + gtsam::Vector3 get_gyroscope_measurement() const; + gtsam::Vector3 get_velocity_measurement() const; + gtsam::Vector3 get_position_measurement() const; + double get_visual_gap_time() const; + gtsam::Rot3 find_current_pose_for_dvl_vel(double time_stamp) const; // Setters void set_depth_measurement(double W_measurement_z); @@ -144,7 +149,7 @@ class Posegraph { void set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S); void set_velocity_measurement(gtsam::Vector3 B_velocity_D); void set_position_measurement(gtsam::Vector3 W_position_C); - void set_visual_gap_time(double visualGapTime); + void set_visual_gap_time(double visual_gap_time); }; } // namespace localization diff --git a/mrobosub_localization_cpp/src/Posegraph.cpp b/mrobosub_localization_cpp/src/Posegraph.cpp new file mode 100644 index 00000000..b7543ab8 --- /dev/null +++ b/mrobosub_localization_cpp/src/Posegraph.cpp @@ -0,0 +1,115 @@ +#include "Posegraph.h" + +#include +#include +#include + +namespace localization { + // Ctor + Dtor + Posegraph::Posegraph() + : _index(0) + , _graph(new gtsam::NonlinearFactorGraph()) + , _initial(new gtsam::Values()) + , _result(new gtsam::Values()) + , _preintegrated_velocity_measurements(new PreintegratedVelocityMeasurementsDvlOnly()) + , _prior_imu_bias(gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0))) { + } + + Posegraph::~Posegraph() {} + + void Posegraph::initialize_parameters(std::shared_ptr node) { + _pose_graph_params = std::make_unique(node); + } + + // Getters + double Posegraph::get_depth_measurement() const { + return _W_measurement_z; + } + + gtsam::Vector3 Posegraph::get_accelerometer_measurement() const { + return _B_accelerometer_S; + } + + gtsam::Vector3 Posegraph::get_gyroscope_measurement() const { + return _B_gyroscope_S; + } + + gtsam::Vector3 Posegraph::get_velocity_measurement() const{ + return _B_velocity_D; + } + + gtsam::Vector3 Posegraph::get_position_measurement() const { + return _W_position_C; + } + + double Posegraph::get_visual_gap_time() const { + return _visual_gap_time; + } + + gtsam::Rot3 Posegraph::find_current_pose_for_dvl_vel(double time_stamp) const { + // find the difference between previous time and current time + gtsam::Pose3 current_keyframe_pose = _prev_pose; + double current_time_diff = time_stamp - _prev_keyframe_time; + double min_diff = std::numeric_limits::max(); + + auto minimum = std::accumulate( + std::begin(_current_dvl_local_timestamps), + std::end(_current_dvl_local_timestamps), + std::make_tuple(0, std::numeric_limits::max(), -1), + [&time_stamp] (std::tuple mins, double next_time_stamp) { + auto [index, min_diff, min_index] = mins; + double difference = abs(time_stamp - next_time_stamp); + bool less = difference < min_diff; + return std::make_tuple( + index + 1, + (less ? difference : min_diff), + (less ? index : min_index) + ); + } + ); + + double min_time_diff; + int min_index; + std::tie(std::ignore, min_time_diff, min_index) = minimum; + + gtsam::Rot3 accumulated_rotation = gtsam::Rot3(); + if (current_time_diff < min_time_diff) { + return accumulated_rotation; + } else { + return std::accumulate( + std::begin(_current_dvl_poses), + std::end(_current_dvl_poses), + accumulated_rotation, + [] (gtsam::Rot3 accumulated_rotation, gtsam::Pose3 dvl_pose) { + return accumulated_rotation * dvl_pose.rotation(); + } + ); + } + } + + // Setters + void Posegraph::set_depth_measurement(double W_measurement_z) { + _W_measurement_z = W_measurement_z; + } + + void Posegraph::set_accelerometer_measurement(gtsam::Vector3 B_accelerometer_S) { + _B_accelerometer_S = B_accelerometer_S; + } + + void Posegraph::set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S) { + _B_gyroscope_S = B_gyroscope_S; + } + + void Posegraph::set_velocity_measurement(gtsam::Vector3 B_velocity_D) { + _B_velocity_D = B_velocity_D; + } + + void Posegraph::set_position_measurement(gtsam::Vector3 W_position_C) { + _W_position_C = W_position_C; + } + + void Posegraph::set_visual_gap_time(double visual_gap_time) { + _visual_gap_time = visual_gap_time; + } + +} // namespace localization \ No newline at end of file From db53a41e1d278b36a82bbcb22ad89f41160482f0 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 1 Apr 2026 07:24:14 +0000 Subject: [PATCH 19/31] Complete the migration of the Posegraph functions --- .../BluerovBarometerFactor.h | 2 +- .../mrobosub_localization_cpp/Posegraph.h | 64 +- .../src/BluerovBarometerFactor.cpp | 12 +- mrobosub_localization_cpp/src/Posegraph.cpp | 728 ++++++++++++++++-- 4 files changed, 699 insertions(+), 107 deletions(-) diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h index 1e83f72a..d22f0acb 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -15,7 +15,7 @@ class BluerovBarometerFactor : public gtsam::NoiseModelFactor1 { public: BluerovBarometerFactor(); - virtual ~BluerovBarometerFactor(); + ~BluerovBarometerFactor(); BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model); gtsam::Vector evaluate_error( diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h index 6ccc7628..f9418efa 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -7,16 +7,20 @@ #include // Matrix44 #include // Vector +#include // Symbol +#include // Key #include // Pose3 #include // Rot3 #include // NavState #include // PreintegratedCombinedMeasurements #include // imuBias::ConstantFactor #include // ISAM2Params -#include // BatchFixedLabSmoother -#include // FixedLagSmoother::KeyTimestampMap +#include // LevenbergMarquardtParams, LevenbergMarquardtOptimizer #include // NonlinearFactorGraph #include // Values +#include // BatchFixedLabSmoother +#include // FixedLagSmoother::KeyTimestampMap +#include // BetweenFactor #include #include @@ -35,13 +39,12 @@ class Posegraph { gtsam::FixedLagSmoother::KeyTimestampMap _smoother_timestamps; // GTSAM - // TODO: Use boost::unique_ptr? boost::shared_ptr _graph; boost::shared_ptr _initial; boost::shared_ptr _result; boost::shared_ptr _preintegrated_measurements; // used to be pim boost::shared_ptr _preintegrated_velocity_measurements; // used to be pvm - boost::shared_ptr _preintergrated_measurement_params; // used to be pim_params + boost::shared_ptr _preintegrated_measurement_params; // used to be pim_params std::unique_ptr _pose_graph_params; // use to be params // Prior imuBias @@ -58,23 +61,25 @@ class Posegraph { gtsam::Vector3 _prev_vel; gtsam::NavState _prev_state; double _prev_keyframe_time; // OLD TODO: used for saving the previous keyframe time + double _prev_dvl_odometry_time; + gtsam::Rot3 _prev_dvl_odometry_rot; // Current - gtsam::Pose3 _current_pose; - gtsam::Vector3 _current_vel; - gtsam::NavState _current_state; - gtsam::imuBias::ConstantBias _current_imu_bias; - double _current_time; - - std::vector _current_dvl_vels; - std::vector _current_dvl_poses; - std::vector _current_dvl_rotations; - std::vector _current_dvl_foms; // TODO: What does FOM mean? + gtsam::Pose3 _curr_pose; + gtsam::Vector3 _curr_vel; + gtsam::NavState _curr_state; + gtsam::imuBias::ConstantBias _curr_imu_bias; + double _curr_time; + + std::vector _curr_dvl_vels; + std::vector _curr_dvl_poses; + std::vector _curr_dvl_rotations; + std::vector _curr_dvl_foms; // TODO: What does FOM mean? std::vector _imu_rot_list; gtsam::Rot3 _imu_prev_rot; - std::vector _current_dvl_timestamps; - std::vector _current_dvl_local_timestamps; + std::vector _curr_dvl_timestamps; + std::vector _curr_dvl_local_timestamps; private: // Members std::mt19937 _rng; @@ -92,15 +97,12 @@ class Posegraph { Posegraph(); ~Posegraph(); - // Initialize parameters - void initialize_parameters(std::shared_ptr node); - // Depth factors - void add_depth_factor(); + // Barometer factors + void add_barometric_factor(double W_measurement_z, double measurement_noise, int baro_id); // IMU factors void add_imu_factor(); - void set_imu_params(); // DVL Factors void add_velocity_factor(); @@ -116,8 +118,7 @@ class Posegraph { // Other factors void add_visual_constraint_factor(gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx); - void add_sonar_factor(); - void add_prior_factor(gtsam::Pose3 initial_pose, gtsam::Vector initial_vec, double pose_noise); + void add_prior_factor(gtsam::Pose3 initial_pose, gtsam::Vector initial_vel, double pose_noise); // Creating transforms void define_transforms(); @@ -132,7 +133,9 @@ class Posegraph { void initialize_pose_graph_from_imu(gtsam::Rot3 initial_rotation); void optimize_pose_graph(); void optimize_pose_graph_smoother(); - void add_edges_to_graph(const std::vector> &edges); + + // Properties + gtsam::Rot3 find_current_pose_for_dvl_vel(double time_stamp) const; // Getters double get_depth_measurement() const; @@ -141,7 +144,6 @@ class Posegraph { gtsam::Vector3 get_velocity_measurement() const; gtsam::Vector3 get_position_measurement() const; double get_visual_gap_time() const; - gtsam::Rot3 find_current_pose_for_dvl_vel(double time_stamp) const; // Setters void set_depth_measurement(double W_measurement_z); @@ -150,6 +152,18 @@ class Posegraph { void set_velocity_measurement(gtsam::Vector3 B_velocity_D); void set_position_measurement(gtsam::Vector3 W_position_C); void set_visual_gap_time(double visual_gap_time); + +private: // Methods + void initialize_parameters(std::shared_ptr node); + void set_imu_parameters(); + void set_smoother_parameters(); + + // Helper methods used for creating the dvl factors + void calculate_interpolated_rotations(bool is_using_slerp, std::vector &interpolated_rotations); + void calculate_integrated_pose_rotation(gtsam::Rot3 &integrated_pose_rotation); + void reset_dvl_odometry(); + void reset_imu_rotation(); + void clear_dvl_timestamp_pose_vel(); }; } // namespace localization diff --git a/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp index f257c470..e7bc78ec 100644 --- a/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp +++ b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp @@ -12,16 +12,12 @@ gtsam::Vector BluerovBarometerFactor::evaluate_error( const gtsam::Pose3 &pose, boost::optional H ) const { - gtsam::Matrix tH; - gtsam::Vector ret = - (gtsam::Vector(1) << (pose.translation(tH).z() - _measured)).finished(); - if (H) { - *H = tH.block<1, 6>(2, 0); + gtsam::Matrix36 Jt; + pose.translation(Jt); + *H = Jt.row(2); } - double error = (pose.z() - _measured); - - return (gtsam::Vector() << error).finished(); + return (gtsam::Vector(1) << pose.z() - _measured).finished(); } } // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/Posegraph.cpp b/mrobosub_localization_cpp/src/Posegraph.cpp index b7543ab8..0ccd45d4 100644 --- a/mrobosub_localization_cpp/src/Posegraph.cpp +++ b/mrobosub_localization_cpp/src/Posegraph.cpp @@ -1,115 +1,697 @@ #include "Posegraph.h" +#include "BluerovBarometerFactor.h" +#include "DvlOnlyFactor.h" + #include #include #include namespace localization { - // Ctor + Dtor - Posegraph::Posegraph() - : _index(0) - , _graph(new gtsam::NonlinearFactorGraph()) - , _initial(new gtsam::Values()) - , _result(new gtsam::Values()) - , _preintegrated_velocity_measurements(new PreintegratedVelocityMeasurementsDvlOnly()) - , _prior_imu_bias(gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0))) { - } +// Ctor + Dtor +Posegraph::Posegraph() + : _index(0) + , _graph(new gtsam::NonlinearFactorGraph()) + , _initial(new gtsam::Values()) + , _result(new gtsam::Values()) + , _preintegrated_velocity_measurements(new PreintegratedVelocityMeasurementsDvlOnly()) + , _prior_imu_bias(gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0))) + , _prev_dvl_odometry_time(0.0) + , _prev_dvl_odometry_rot(gtsam::Rot3()) { + // Set other parameters + set_smoother_parameters(); + set_imu_parameters(); +} + +Posegraph::~Posegraph() {} + +void Posegraph::initialize_parameters(std::shared_ptr node) { + _pose_graph_params = std::make_unique(node); +} + +void Posegraph::set_smoother_parameters() { + _smoother_parameters.relinearizeThreshold = 0.01; + _smoother_parameters.relinearizeSkip = 1; + _smoother_ISAM2 = gtsam::BatchFixedLagSmoother(_smoother_lag); +} + +void Posegraph::set_imu_parameters() { + _preintegrated_measurement_params = boost::make_shared( + gtsam::Vector3(0, 0, _pose_graph_params->_imu_params.g) + ); + _preintegrated_measurement_params->setAccelerometerCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.acc_noise_density, 2) + ); + _preintegrated_measurement_params->setGyroscopeCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.gyro_noise_density, 2) + ); + _preintegrated_measurement_params->setIntegrationCovariance( + gtsam::I_3x3 * 1e-8 + ); + _preintegrated_measurement_params->setBiasAccCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.acc_random_walk, 2) + ); + _preintegrated_measurement_params->setBiasOmegaCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.gyro_random_walk, 2) + ); + + double prior_acc_bias = _pose_graph_params->_imu_params.acc_bias_prior; + double prior_gyro_bias = _pose_graph_params->_imu_params.gyro_bias_prior; + double prior_dvl_bias = _pose_graph_params->_dvl_params.prior_bias; + + gtsam::Vector3 prior_acc_bias_vec = (gtsam::Vector(3) << prior_acc_bias, prior_acc_bias, prior_acc_bias).finished(); + gtsam::Vector3 prior_gyro_bias_vec = (gtsam::Vector(3) << prior_gyro_bias, prior_gyro_bias, prior_gyro_bias).finished(); + gtsam::Vector3 prior_dvl_bias_vec = (gtsam::Vector(3) << prior_dvl_bias, prior_dvl_bias, prior_dvl_bias).finished(); + gtsam::Vector3 prior_zeros_vec = (gtsam::Vector(3) << 0, 0, 0).finished(); - Posegraph::~Posegraph() {} + _prior_imu_bias = gtsam::imuBias::ConstantBias(prior_acc_bias_vec, prior_gyro_bias_vec); + _prior_dvl_bias = gtsam::imuBias::ConstantBias(prior_dvl_bias_vec, prior_zeros_vec); - void Posegraph::initialize_parameters(std::shared_ptr node) { - _pose_graph_params = std::make_unique(node); + _preintegrated_measurements = boost::make_shared( + _preintegrated_measurement_params, + _prior_imu_bias + ); +} + +void Posegraph::add_barometric_factor(double W_measurement_z, double measurement_noise, int baro_id) { + _graph->emplace_shared( + gtsam::Symbol('x', baro_id), + W_measurement_z, + gtsam::noiseModel::Isotropic::Sigma(1, measurement_noise) + ); +} + +void Posegraph::add_imu_factor() { + _graph->emplace_shared( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('v', _index - 1), + gtsam::Symbol('x', _index), + gtsam::Symbol('v', _index), + gtsam::Symbol('b', _index - 1), + gtsam::Symbol('b', _index), + _preintegrated_measurement_params.get() + ); +} + +void Posegraph::add_velocity_factor_with_rotation_interpolation(bool using_slerp) { + // Get the size of the current dvl measurements + size_t dvl_size = _curr_dvl_timestamps.size(); + size_t dvl_local_size = _curr_dvl_local_timestamps.size(); + + // If any of the size is zero, assert error + if (dvl_size == 0 && dvl_local_size == 0) { + assert(false); } - // Getters - double Posegraph::get_depth_measurement() const { - return _W_measurement_z; + std::vector interpolated_rotations; + calculate_interpolated_rotations(using_slerp, interpolated_rotations); + + // Calculate the integrated pose translation by accumulating dvl velocities with the right rotation for each + // timestamp we have + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + double dt_dvl = -1; + gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); + for (size_t i = 0; i < dvl_size; ++i) { + dt_dvl = (dt_dvl == -1) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i-1]; + integrated_rot_matrix = integrated_rot_matrix * interpolated_rotations[i].matrix(); + integrated_pose_translation += integrated_rot_matrix * _curr_dvl_vels[i] * dt_dvl; } - gtsam::Vector3 Posegraph::get_accelerometer_measurement() const { - return _B_accelerometer_S; + // Calculate the integrated pose rotation by accumulating rotations + gtsam::Rot3 integrated_pose_rotation; + calculate_integrated_pose_rotation(integrated_pose_rotation); + + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + // Insert the integrated velocity pose into the graph + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(0.01) + ) + ); } - gtsam::Vector3 Posegraph::get_gyroscope_measurement() const { - return _B_gyroscope_S; + // clear variables used above + reset_dvl_odometry(); + clear_dvl_timestamp_pose_vel(); +} + +void Posegraph::add_dvl_factor(bool using_slerp) { + // Get the size of the current dvl measurements + size_t dvl_size = _curr_dvl_timestamps.size(); + size_t dvl_local_size = _curr_dvl_local_timestamps.size(); + + // If any of the size is zero, assert error + if (dvl_size == 0 && dvl_local_size == 0) { + assert(false); } - gtsam::Vector3 Posegraph::get_velocity_measurement() const{ - return _B_velocity_D; + std::vector interpolated_rotations; + calculate_interpolated_rotations(using_slerp, interpolated_rotations); + + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + double dt_dvl = -1; + gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); + for (size_t i = 0; i < dvl_size; ++i) { + dt_dvl = (dt_dvl == -1) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i-1]; + integrated_rot_matrix = integrated_rot_matrix * interpolated_rotations[i].matrix(); + _preintegrated_velocity_measurements->integrate_measurements( + _curr_dvl_vels[i], gtsam::Rot3(integrated_rot_matrix), dt_dvl + ); + integrated_pose_translation += integrated_rot_matrix * _curr_dvl_vels[i] * dt_dvl; + std::cout << "[add_dvl_factor] dt_dvl: " << dt_dvl << "\n"; + std::cout << "[add_dvl_factor] _curr_dvl_vels[i]: " << _curr_dvl_vels[i] << "\n"; + std::cout << "[add_dvl_factor] integrated_pose_translation: " << integrated_pose_translation << "\n"; } - gtsam::Vector3 Posegraph::get_position_measurement() const { - return _W_position_C; + gtsam::Rot3 integrated_pose_rotation; + calculate_integrated_pose_rotation(integrated_pose_rotation); + + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(0.01) + ) + ); + _graph->emplace_shared( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + gtsam::Symbol('d', _index), + *_preintegrated_velocity_measurements + ); + _graph->emplace_shared>( + gtsam::Symbol('d', _index - 1), + gtsam::Symbol('d', _index), + gtsam::imuBias::ConstantBias(gtsam::Vector3(0, 0, 0), gtsam::Vector(0, 0, 0)), + gtsam::noiseModel::Isotropic::Sigma(6, 1e-3) + ); } - double Posegraph::get_visual_gap_time() const { - return _visual_gap_time; + reset_dvl_odometry(); + clear_dvl_timestamp_pose_vel(); + _preintegrated_velocity_measurements->reset_integration(); +} + +void Posegraph::add_dvl_factor_imu_rotation() { + int dvl_size = _curr_dvl_timestamps.size(); + assert(_curr_dvl_vels.size() == _imu_rot_list.size()); + + double dt_dvl; + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); + + for (size_t i = 0; i < dvl_size; ++i) { + dt_dvl = (dt_dvl == -1) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i-1]; + integrated_rot_matrix = _imu_prev_rot.matrix().inverse() * _imu_rot_list[i].matrix(); + _preintegrated_velocity_measurements->integrate_measurements_noise( + _curr_dvl_vels[i], + gtsam::Rot3(integrated_rot_matrix), + dt_dvl, + _curr_dvl_foms[i] + ); + integrated_pose_translation += integrated_rot_matrix.matrix() * _curr_dvl_vels[i] * dt_dvl; } + gtsam::Rot3 integrated_pose_rotation = _imu_prev_rot.inverse() * _imu_rot_list.back(); + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); - gtsam::Rot3 Posegraph::find_current_pose_for_dvl_vel(double time_stamp) const { - // find the difference between previous time and current time - gtsam::Pose3 current_keyframe_pose = _prev_pose; - double current_time_diff = time_stamp - _prev_keyframe_time; - double min_diff = std::numeric_limits::max(); + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + if (_index < 10000) { + _graph->emplace_shared( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + gtsam::Symbol('d', _index - 1), + *_preintegrated_velocity_measurements + ); - auto minimum = std::accumulate( - std::begin(_current_dvl_local_timestamps), - std::end(_current_dvl_local_timestamps), - std::make_tuple(0, std::numeric_limits::max(), -1), - [&time_stamp] (std::tuple mins, double next_time_stamp) { - auto [index, min_diff, min_index] = mins; - double difference = abs(time_stamp - next_time_stamp); - bool less = difference < min_diff; - return std::make_tuple( - index + 1, - (less ? difference : min_diff), - (less ? index : min_index) - ); - } + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.1, 0.1, 0.1).finished() + ) + ); + } else { + std::cout << "[add_dvl_factor_imu_rotation] no dvl factor added as index greater than 10,000\n"; + } + + _graph->emplace_shared>( + gtsam::Symbol('d', _index - 1), + gtsam::Symbol('d', _index), + gtsam::imuBias::ConstantBias(gtsam::Vector3(0, 0, 0), gtsam::Vector(0, 0, 0)), + gtsam::noiseModel::Isotropic::Sigma(6, 1e-1) ); + } + + clear_dvl_timestamp_pose_vel(); + _curr_dvl_foms.clear(); + reset_imu_rotation(); + _preintegrated_velocity_measurements->reset_integration(); +} + +void Posegraph::add_velocity_factor() { + int dvl_size = _curr_dvl_timestamps.size(); + int dvl_local_size = _curr_dvl_local_timestamps.size(); + + assert(dvl_size > 0); + + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + double dt; + for (size_t i = 0; i < dvl_size; ++i) { + dt = (i == 0) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i - 1]; - double min_time_diff; - int min_index; - std::tie(std::ignore, min_time_diff, min_index) = minimum; + gtsam::Rot3 curr_rotation = find_current_pose_for_dvl_vel(_curr_dvl_timestamps[i]); + gtsam::Vector3 curr_velocity = _curr_dvl_vels[i]; + gtsam::Vector3 curr_velocity_world = curr_rotation.matrix() * curr_velocity; + integrated_pose_translation += curr_velocity_world * dt; + } - gtsam::Rot3 accumulated_rotation = gtsam::Rot3(); - if (current_time_diff < min_time_diff) { - return accumulated_rotation; - } else { - return std::accumulate( - std::begin(_current_dvl_poses), - std::end(_current_dvl_poses), - accumulated_rotation, - [] (gtsam::Rot3 accumulated_rotation, gtsam::Pose3 dvl_pose) { - return accumulated_rotation * dvl_pose.rotation(); - } - ); + gtsam::Rot3 integrated_pose_rotation; + calculate_integrated_pose_rotation(integrated_pose_rotation); + + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(0.01) + ) + ); + } + + // clear variables used above + clear_dvl_timestamp_pose_vel(); +} + +void Posegraph::add_dvl_odometry_factor(double noise) { + gtsam::Pose3 between_pose = std::accumulate( + std::begin(_curr_dvl_poses), + std::end(_curr_dvl_poses), + gtsam::Pose3(), + [] (gtsam::Pose3 total, gtsam::Pose3 pose) { + return total * pose; } + ); + if (_index > 0) { + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + between_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(noise) + ) + ); } - - // Setters - void Posegraph::set_depth_measurement(double W_measurement_z) { - _W_measurement_z = W_measurement_z; +} + +void Posegraph::add_dvl_velocity(double time_stamp, gtsam::Vector3 velocity) { + _curr_dvl_timestamps.push_back(time_stamp); + _curr_dvl_vels.push_back(velocity); +} +void Posegraph::add_dvl_pose(double time_stamp, gtsam::Pose3 pose) { + _curr_dvl_local_timestamps.push_back(time_stamp); + _curr_dvl_poses.push_back(pose); +} +void Posegraph::add_dvl_rotation(double time_stamp, gtsam::Rot3 rotation) { + _curr_dvl_local_timestamps.push_back(time_stamp); + _curr_dvl_rotations.push_back(rotation); +} + + +void Posegraph::add_visual_constraint_factor(gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx) { + _graph->emplace_shared>( + gtsam::Symbol('x', prev_idx), + gtsam::Symbol('x', curr_idx), + between_pose, + gtsam::noiseModel::Gaussian::Information( + gtsam::Matrix66::Identity() * weight * 1e-6 + ) + ); +} + + +void Posegraph::add_prior_factor(gtsam::Pose3 initial_pose, gtsam::Vector initial_vel, double pose_noise) { + _graph->emplace_shared>( + gtsam::Symbol('x', 0), + initial_pose, + gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << pose_noise, pose_noise, pose_noise, pose_noise, pose_noise, pose_noise).finished() + ) + ); + _graph->emplace_shared>( + gtsam::Symbol('v', 0), + initial_vel, + gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(3) << pose_noise, pose_noise, pose_noise).finished() + ) + ); +} + +// Creating transforms +void Posegraph::define_transforms() { + _T_SD = _pose_graph_params->_extrinsics.T_SD; + _T_SB = _pose_graph_params->_extrinsics.T_BS; + _T_W_WD = _pose_graph_params->_extrinsics.T_W_WD; +} + +// Adding estimates +void Posegraph::add_simple_estimate(double dt, double noise) { + // Create a random vector of dim 3 + gtsam::Vector3 noisy = gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)) * noise; + if (_pose_graph_params->_sensor_usage.is_imu_used) { + gtsam::Pose3 pose_i = _initial->at(gtsam::Symbol('x', _index - 1)); + gtsam::Vector3 vel_i = _initial->at(gtsam::Symbol('v', _index - 1)); + gtsam::imuBias::ConstantBias bias_i = _initial->at(gtsam::Symbol('b', _index - 1)); + + // Compute translation update + gtsam::Vector3 W_dp_S = pose_i.rotation().matrix() * (vel_i * dt + 0.5 * std::pow(dt, 2) * noisy); + gtsam::Vector3 W_p_S = pose_i.translation() + W_dp_S; + + // Compute rotation update + // Perturb the rotation with a small angle about a random axis + gtsam::Vector3 axis = gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)); + axis.normalize(); + double angle = _normal_distribution(_rng); + gtsam::Rot3 R_W_S = gtsam::Rot3::Rodrigues(axis * angle) * pose_i.rotation(); + + // Compute pose + gtsam::Pose3 pred_pose(R_W_S, W_p_S); + + // Compute velocity update + gtsam::Vector3 B_v_S = vel_i + dt * noisy; + gtsam::Vector3 &pred_vel = B_v_S; + + // Create a bias by perturbing the prior imu bias + gtsam::imuBias::ConstantBias pred_bias( + _prior_imu_bias.gyroscope() + gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)), + _prior_imu_bias.accelerometer() + gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)) + ); + + // Push to initial + _initial->insert(gtsam::Symbol('x', _index), pred_pose); + _initial->insert(gtsam::Symbol('v', _index), pred_vel); + _initial->insert(gtsam::Symbol('b', _index), pred_bias); + _initial->insert(gtsam::Symbol('d', _index), _prior_dvl_bias); } +} - void Posegraph::set_accelerometer_measurement(gtsam::Vector3 B_accelerometer_S) { - _B_accelerometer_S = B_accelerometer_S; +void Posegraph::add_initial_estimate(gtsam::Pose3 initial_pose, gtsam::Vector3 intitial_vel) { + _initial->insert(gtsam::Symbol('x', _index), initial_pose); + if (_pose_graph_params->_sensor_usage.is_imu_used) { + _initial->insert(gtsam::Symbol('b', _index), _prior_imu_bias); + } + if (_pose_graph_params->_sensor_usage.is_dvl_used) { + _initial->insert(gtsam::Symbol('d', _index), _prior_dvl_bias); } +} - void Posegraph::set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S) { - _B_gyroscope_S = B_gyroscope_S; +void Posegraph::add_visual_estimate(const std::vector vertex) { + Eigen::Quaterniond quaternion(vertex[8], vertex[5], vertex[5], vertex[7]); + Eigen::Vector3d translation(vertex[2], vertex[3], vertex[4]); + Eigen::Vector3d velocity(vertex[9], vertex[10], vertex[11]); + Eigen::Vector3d bias_acc(vertex[12], vertex[13], vertex[14]); + Eigen::Vector3d bias_gyro(vertex[15], vertex[16], vertex[17]); + + // Convert quaternion to Rot3 + gtsam::Rot3 R(quaternion.normalized().toRotationMatrix()); + + // Convert translation to Point3 + gtsam::Point3 p(translation); + + // Create Pose 3 + gtsam::Pose3 W_visual_estimated_pose(R, p); + gtsam::imuBias::ConstantBias visual_estimated_bias(bias_acc, bias_gyro); + gtsam::Point3 B_visual_esimate_velocity(velocity); + + // Set previous pose to be the estimated one + _prev_pose = W_visual_estimated_pose; + + // Push to initial + _initial->insert(gtsam::Symbol('x', _index), W_visual_estimated_pose); + if (_pose_graph_params->_sensor_usage.is_imu_used) { + _initial->insert(gtsam::Symbol('v', _index), B_visual_esimate_velocity); + _initial->insert(gtsam::Symbol('b', _index), visual_estimated_bias); + } + if (_pose_graph_params->_sensor_usage.is_dvl_used) { + _initial->insert(gtsam::Symbol('d', _index), _prior_dvl_bias); } +} - void Posegraph::set_velocity_measurement(gtsam::Vector3 B_velocity_D) { - _B_velocity_D = B_velocity_D; +// Working with the pose graph +void Posegraph::initialize_pose_graph() { + // If we are not using orbslam, it is okay to keep pose from identity + gtsam::Pose3 prior_pose = gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0, 0, 0)); + if (_pose_graph_params->_using_orbslam) { + assert(false); // TODO } - void Posegraph::set_position_measurement(gtsam::Vector3 W_position_C) { - _W_position_C = W_position_C; + add_initial_estimate(prior_pose, gtsam::Vector3()); + _prev_pose = prior_pose; + add_prior_factor(prior_pose, gtsam::Vector3(), 0.001); +} + +void Posegraph::initialize_pose_graph_from_imu(gtsam::Rot3 initial_rotation) { + gtsam::Pose3 prior_pose = gtsam::Pose3(initial_rotation, gtsam::Point3(0, 0, 0)); + add_initial_estimate(prior_pose, gtsam::Vector3()); + _prev_pose = prior_pose; + add_prior_factor(prior_pose, gtsam::Vector3(), 0.001); +} + +void Posegraph::optimize_pose_graph() { + gtsam::LevenbergMarquardtParams lm_params; + gtsam::LevenbergMarquardtOptimizer optimizer(*_graph, *_initial, lm_params); + + *_result = optimizer.optimize(); + std::cout << "[optimize_pose_graph] Initial error = " << _graph->error(*_initial) << "\n"; + *_initial = *_result; + std::cout << "[optimize_pose_graph] Final error = " << _graph->error(*_result) << "\n"; +} + +void Posegraph::optimize_pose_graph_smoother() { + gtsam::FactorIndices delete_slots; + _smoother_ISAM2.update(*_graph, *_initial, _smoother_timestamps, delete_slots); + *_result = _smoother_ISAM2.calculateEstimate(); + + // Calculate error + double error = _smoother_ISAM2.getFactors().error(*_result); + if (std::isnan(error)) { + std::cout << "[optimize_pose_graph_smoother] Error is NaN \n"; + assert(false); } + std::cout << "[optimize_pose_graph_smoother] Final error = " << _smoother_ISAM2.getFactors().error(*_result) << "\n"; + + _smoother_timestamps.clear(); + _initial->clear(); + _graph->resize(0); +} + +gtsam::Rot3 Posegraph::find_current_pose_for_dvl_vel(double time_stamp) const { + // find the difference between previous time and current time + gtsam::Pose3 current_keyframe_pose = _prev_pose; + double current_time_diff = time_stamp - _prev_keyframe_time; + double min_diff = std::numeric_limits::max(); + + auto minimum = std::accumulate( + std::begin(_curr_dvl_local_timestamps), + std::end(_curr_dvl_local_timestamps), + std::make_tuple(0, std::numeric_limits::max(), -1), + [&time_stamp] (std::tuple mins, double next_time_stamp) { + auto [index, min_diff, min_index] = mins; + double difference = abs(time_stamp - next_time_stamp); + bool less = difference < min_diff; + return std::make_tuple( + index + 1, + (less ? difference : min_diff), + (less ? index : min_index) + ); + } + ); + + double min_time_diff; + int min_index; + std::tie(std::ignore, min_time_diff, min_index) = minimum; - void Posegraph::set_visual_gap_time(double visual_gap_time) { - _visual_gap_time = visual_gap_time; + gtsam::Rot3 accumulated_rotation = gtsam::Rot3(); + if (current_time_diff < min_time_diff) { + return accumulated_rotation; + } else { + return std::accumulate( + std::begin(_curr_dvl_poses), + std::end(_curr_dvl_poses), + accumulated_rotation, + [] (gtsam::Rot3 accumulated_rotation, gtsam::Pose3 dvl_pose) { + return accumulated_rotation * dvl_pose.rotation(); + } + ); } +} + +void Posegraph::calculate_interpolated_rotations(bool is_using_slerp, std::vector &interpolated_rotations) { + // Interpolate the rotations + gtsam::Rot3 prev_rotation = gtsam::Rot3(); + gtsam::Rot3 curr_rotation = _curr_dvl_poses[0].rotation(); + double last_local_time = _prev_dvl_odometry_time; + double curr_local_time = _curr_dvl_local_timestamps[0]; + int next_local_idx = 0; + double dt, dt_interpolated; + + // For each dvl timestamp need to interpolate the current rotation + // based off of how far between last_local_time and curr_local_time + // the curr_dvl_timestamp is + for (const double &curr_dvl_timestamp : _curr_dvl_timestamps) { + // If the current dvl timestamp is greater than the current local time + // move to the next current local time and next rotation captured + if (curr_dvl_timestamp > curr_local_time) { + next_local_idx++; + prev_rotation = curr_rotation; // TODO: This was commented out??? + curr_rotation = _curr_dvl_poses[next_local_idx].rotation(); + last_local_time = curr_local_time; + curr_local_time = _curr_dvl_local_timestamps[next_local_idx]; + } + + // Find how far ahead of the last local timestamp this dvl timestamp is + dt = curr_dvl_timestamp - last_local_time; + dt_interpolated = dt / (curr_local_time - last_local_time); + + // The dt_interp needs to be between [0, 1] + // I suppose this could happen if curr_dvl_timestamp is _still_ + // greater than the curr_local_time. + if (dt_interpolated < 0 || dt_interpolated > 1) { + std::cout << "[calculate_interpolated_rotations] interpolated dt outside of [0, 1]\n"; + std::cout << "[calculate_interpolated_rotations] dt: " << dt << "\n"; + std::cout << "[calculate_interpolated_rotations] curr_dvl_timestamp: " << curr_dvl_timestamp << "\n"; + std::cout << "[calculate_interpolated_rotations] last_local_time: " << last_local_time << "\n"; + std::cout << "[calculate_interpolated_rotations] curr_local_time: " << curr_local_time << "\n"; + std::cout << "[calculate_interpolated_rotations] curr_rotation: " << curr_rotation << "\n"; + std::cout << "[calculate_interpolated_rotations] prev_rotation (NOT SET): " << prev_rotation << "\n"; + assert(false); + } + + gtsam::Rot3 interpolated_rotation; + if (is_using_slerp) { + interpolated_rotation = prev_rotation.slerp(dt_interpolated, curr_rotation); + std::cout << "[calculate_interpolated_rotations] interpolated_rotation: " << interpolated_rotation << "\n"; + std::cout << "[calculate_interpolated_rotations] curr_rotation: " << curr_rotation << "\n"; + std::cout << "[calculate_interpolated_rotations] prev_rotation (NOT SET): " << prev_rotation << "\n"; + std::cout << "-------------------------------------------------\n"; + } else { + assert(false); + } + + interpolated_rotations.push_back(interpolated_rotation); + } + + // Check to see that we have an interpolated rotation for each dvl timestamp + if (_curr_dvl_timestamps.size() != interpolated_rotations.size()) { + std::cout << "[add_velocity_factor_with_rotation_interpolation] dvl_size and interpolated_rotations.size() mismatch\n"; + std::cout << "[add_velocity_factor_with_rotation_interpolation] dvl_size: " << _curr_dvl_timestamps.size() << "\n"; + std::cout << "[add_velocity_factor_with_rotation_interpolation] interpolated_rotations.size(): " << interpolated_rotations.size() << "\n"; + assert(false); + } +} + +void Posegraph::calculate_integrated_pose_rotation(gtsam::Rot3 &integrated_pose_rotation) { + // Calculate the integrated pose rotation by accumulating rotations + integrated_pose_rotation = std::accumulate( + std::begin(_curr_dvl_poses), + std::end(_curr_dvl_poses), + gtsam::Rot3(), + [](gtsam::Rot3 total, gtsam::Pose3 pose) { + return total * pose.rotation(); + } + ); +} + +void Posegraph::reset_dvl_odometry() { + _prev_dvl_odometry_time = _curr_dvl_local_timestamps.back(); + _prev_dvl_odometry_rot = _curr_dvl_poses.back().rotation(); +} + +void Posegraph::clear_dvl_timestamp_pose_vel() { + _curr_dvl_timestamps.clear(); + _curr_dvl_vels.clear(); + _curr_dvl_poses.clear(); + _curr_dvl_local_timestamps.clear(); +} + + +void Posegraph::reset_imu_rotation() { + _imu_prev_rot = _imu_rot_list.back(); + _imu_rot_list.clear(); +} + +// Getters +double Posegraph::get_depth_measurement() const { + return _W_measurement_z; +} + +gtsam::Vector3 Posegraph::get_accelerometer_measurement() const { + return _B_accelerometer_S; +} + +gtsam::Vector3 Posegraph::get_gyroscope_measurement() const { + return _B_gyroscope_S; +} + +gtsam::Vector3 Posegraph::get_velocity_measurement() const{ + return _B_velocity_D; +} + +gtsam::Vector3 Posegraph::get_position_measurement() const { + return _W_position_C; +} + +double Posegraph::get_visual_gap_time() const { + return _visual_gap_time; +} + +// Setters +void Posegraph::set_depth_measurement(double W_measurement_z) { + _W_measurement_z = W_measurement_z; +} + +void Posegraph::set_accelerometer_measurement(gtsam::Vector3 B_accelerometer_S) { + _B_accelerometer_S = B_accelerometer_S; +} + +void Posegraph::set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S) { + _B_gyroscope_S = B_gyroscope_S; +} + +void Posegraph::set_velocity_measurement(gtsam::Vector3 B_velocity_D) { + _B_velocity_D = B_velocity_D; +} + +void Posegraph::set_position_measurement(gtsam::Vector3 W_position_C) { + _W_position_C = W_position_C; +} + +void Posegraph::set_visual_gap_time(double visual_gap_time) { + _visual_gap_time = visual_gap_time; +} } // namespace localization \ No newline at end of file From 3ac80207dfbe563962f95643ede4f1b6a87d693a Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 1 Apr 2026 13:25:04 +0000 Subject: [PATCH 20/31] Change IMU publisher to report both PIMU and INS at the same itme --- mrobosub_hal_imu/src/imu_node.cpp | 115 +++++++++++++++++++----------- mrobosub_msgs/CMakeLists.txt | 1 + mrobosub_msgs/msg/Imu.msg | 6 ++ 3 files changed, 79 insertions(+), 43 deletions(-) create mode 100644 mrobosub_msgs/msg/Imu.msg diff --git a/mrobosub_hal_imu/src/imu_node.cpp b/mrobosub_hal_imu/src/imu_node.cpp index 512ee6b5..0d99277c 100644 --- a/mrobosub_hal_imu/src/imu_node.cpp +++ b/mrobosub_hal_imu/src/imu_node.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -39,6 +40,19 @@ class TimeManager std::shared_ptr nh_; }; +struct PimuStorage { + pimu_t pimu; + bool has_pimu = false; + double tow_offset = -1; +}; + +PimuStorage pimu_storage; + +void global_message_callback(void *ctx, p_data_t *data, port_handle_t port) { + auto node = static_cast(ctx); +} + + int main(int argc, char **argv) { rclcpp::init(argc, argv); @@ -55,64 +69,79 @@ int main(int argc, char **argv) return 1; } - auto pub_ins = node->create_publisher("/imu_INS", 1); - auto pub_pimu = node->create_publisher("/imu_PIMU", 1); - if (!pub_ins || !pub_pimu) - { + auto imu_pub = node->create_publisher("/imu", 1); + if (!imu_pub) { return -1; } - InertialSense is; + // We need to attach a global callback to all DID messages returned by the + // sensor so that we can sync the INS (rotation) and PIMU (linear acc and angular vel) measurements + InertialSense is([&](InertialSense *is_ptr, p_data_t *data, int port_handle) { + switch(data->hdr.id) { + case DID_GPS1_POS: { + auto gps = reinterpret_cast(data->ptr); + pimu.tow_offset = gps->towOffset; + break; + } + + case DID_PIMU: { + pimu_storage.pimu = reinterpret_cast(data->ptr); + pimu_storage.has_pimu = true; + break; + } + + case DID_INS_1: { + if (pimu_storage.has_pimu) return; // We want to sync the DID_PIMU and DID_INS_1 messages. + + auto ins = reinterpret_cast(data->ptr); + + double pimu_time_to_tow = pimu_storage.pimu.time + pimu_storage.tow_offset; + + if (std::abs(pimu_time_to_tow - ins->timeOfWeek) < TIME_EPSILON) { + auto time_stamp = timeManager.ros_time_from_start_time(ins->timeOfWeek); + const auto div = 1.0f/data->dt; + + mrobosub_msgs::msg::Imu msg; + msg.header.stamp = time_stamp; + msg.dt = data->dt; + msg.angular_velocity.x = data->theta[0] * div; + msg.angular_velocity.y = data->theta[1] * div; + msg.angular_velocity.z = data->theta[2] * div; + msg.linear_acceleration.x = data->vel[0] * div; + msg.linear_acceleration.y = data->vel[1] * div; + msg.linear_acceleration.z = data->vel[2] * div; + msg.theta.x = data->theta[0]; + msg.theta.y = data->theta[1]; + msg.theta.z = data->theta[2]; + imu_pub->publish(msg); + } + pimu_storage.has_pimu = false; + break; + } + } + }); + is.Open(non_ros_args[1].c_str()); - auto pimu_registered = is.BroadcastBinaryData( - DID_PIMU, - 1, - [&](InertialSense *is, p_data_t *_data, int pHandle) - { - const auto data = reinterpret_cast(_data->ptr); - - mrobosub_msgs::msg::ImuPIMU msg; - const auto div = 1.0f/data->dt; - - msg.header.stamp = timeManager.ros_time_from_start_time(data->time); - msg.dt = data->dt; - msg.angular_velocity.x = data->theta[0] * div; - msg.angular_velocity.y = data->theta[1] * div; - msg.angular_velocity.z = data->theta[2] * div; - msg.linear_acceleration.x = data->vel[0] * div; - msg.linear_acceleration.y = data->vel[1] * div; - msg.linear_acceleration.z = data->vel[2] * div; - pub_pimu->publish(msg); - } - ); + auto pimu_registered = is.BroadcastBinaryData(DID_PIMU, 1); if (!pimu_registered) { return 1; } - auto ins_registered = is.BroadcastBinaryData( - DID_INS_1, - 1, - [&](InertialSense *is, p_data_t *_data, int pHandle) - { - const auto data = reinterpret_cast(_data->ptr); - - mrobosub_msgs::msg::ImuINS msg; - - msg.header.stamp = timeManager.ros_time_from_start_time(data->timeOfWeek); - msg.theta.x = data->theta[0]; - msg.theta.y = data->theta[1]; - msg.theta.z = data->theta[2]; - pub_ins->publish(msg); - } - ); - + auto ins_registered = is.BroadcastBinaryData(DID_INS_1, 1); if (!ins_registered) { return 1; } + + // Needed to get the offset to time of week + auto gps_registered = is.BroadcastBinaryData(DID_GPS1_POS, 1); + if (!gps_registered) + { + return 1; + } while (rclcpp::ok()) { diff --git a/mrobosub_msgs/CMakeLists.txt b/mrobosub_msgs/CMakeLists.txt index 6460e9a0..37d77be0 100644 --- a/mrobosub_msgs/CMakeLists.txt +++ b/mrobosub_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Detection.msg" "msg/Detections.msg" "msg/Dvl.msg" + "msg/Imu.msg" "msg/ImuINS.msg" "msg/ImuPIMU.msg" "msg/MotorState.msg" diff --git a/mrobosub_msgs/msg/Imu.msg b/mrobosub_msgs/msg/Imu.msg new file mode 100644 index 00000000..85fb8715 --- /dev/null +++ b/mrobosub_msgs/msg/Imu.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +geometry_msgs/Vector3 angular_velocity +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Vector3 theta # angle (rodriguez vector) +float64 dt # Time difference since last measurement in seconds + # (Used to calculate velocity and accel from diffs) \ No newline at end of file From 76bed33d0b47d8de8c9989302b232d9c25d071df Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Mon, 13 Apr 2026 08:34:24 +0000 Subject: [PATCH 21/31] Change from publishing euler angles to quaternions --- .vscode/c_cpp_properties.json | 2 +- mrobosub_hal_imu/src/imu_node.cpp | 41 +++++------ .../mrobosub_localization/localization.py | 73 +++++++++++-------- mrobosub_msgs/msg/Imu.msg | 6 +- 4 files changed, 64 insertions(+), 58 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 51d8508a..c6811d87 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -13,7 +13,7 @@ "defines": [], "compilerPath": "/usr/bin/gcc", "cStandard": "gnu17", - "cppStandard": "gnu++14", + "cppStandard": "c++17", "intelliSenseMode": "linux-gcc-x64" } ], diff --git a/mrobosub_hal_imu/src/imu_node.cpp b/mrobosub_hal_imu/src/imu_node.cpp index 0d99277c..6ee92c74 100644 --- a/mrobosub_hal_imu/src/imu_node.cpp +++ b/mrobosub_hal_imu/src/imu_node.cpp @@ -6,8 +6,6 @@ #include "../inertial-sense-sdk/src/data_sets.h" #include -#include -#include #include #include @@ -48,11 +46,10 @@ struct PimuStorage { PimuStorage pimu_storage; -void global_message_callback(void *ctx, p_data_t *data, port_handle_t port) { - auto node = static_cast(ctx); -} - - +/** + * Publishes: + * /imu Imu + */ int main(int argc, char **argv) { rclcpp::init(argc, argv); @@ -75,7 +72,7 @@ int main(int argc, char **argv) } // We need to attach a global callback to all DID messages returned by the - // sensor so that we can sync the INS (rotation) and PIMU (linear acc and angular vel) measurements + // sensor so that we can sync the INS (orientation) and PIMU (linear acc and angular vel) measurements InertialSense is([&](InertialSense *is_ptr, p_data_t *data, int port_handle) { switch(data->hdr.id) { case DID_GPS1_POS: { @@ -89,11 +86,10 @@ int main(int argc, char **argv) pimu_storage.has_pimu = true; break; } + case DID_INS_2: { + if (!pimu_storage.has_pimu) return; // We want to sync the DID_PIMU and DID_INS_2 messages. - case DID_INS_1: { - if (pimu_storage.has_pimu) return; // We want to sync the DID_PIMU and DID_INS_1 messages. - - auto ins = reinterpret_cast(data->ptr); + auto ins = reinterpret_cast(data->ptr); double pimu_time_to_tow = pimu_storage.pimu.time + pimu_storage.tow_offset; @@ -104,15 +100,16 @@ int main(int argc, char **argv) mrobosub_msgs::msg::Imu msg; msg.header.stamp = time_stamp; msg.dt = data->dt; - msg.angular_velocity.x = data->theta[0] * div; - msg.angular_velocity.y = data->theta[1] * div; - msg.angular_velocity.z = data->theta[2] * div; - msg.linear_acceleration.x = data->vel[0] * div; - msg.linear_acceleration.y = data->vel[1] * div; - msg.linear_acceleration.z = data->vel[2] * div; - msg.theta.x = data->theta[0]; - msg.theta.y = data->theta[1]; - msg.theta.z = data->theta[2]; + msg.angular_velocity.x = pimu_storage.pimu.theta[0] * div; + msg.angular_velocity.y = pimu_storage.pimu.theta[1] * div; + msg.angular_velocity.z = pimu_storage.pimu.theta[2] * div; + msg.linear_acceleration.x = pimu_storage.pimu.vel[0] * div; + msg.linear_acceleration.y = pimu_storage.pimu.vel[1] * div; + msg.linear_acceleration.z = pimu_storage.pimu.vel[2] * div; + msg.theta.w = data->theta[0]; + msg.theta.x = data->theta[1]; + msg.theta.y = data->theta[2]; + msg.theta.z = data->theta[3]; imu_pub->publish(msg); } pimu_storage.has_pimu = false; @@ -130,7 +127,7 @@ int main(int argc, char **argv) return 1; } - auto ins_registered = is.BroadcastBinaryData(DID_INS_1, 1); + auto ins_registered = is.BroadcastBinaryData(DID_INS_2, 1); if (!ins_registered) { return 1; diff --git a/mrobosub_localization/mrobosub_localization/localization.py b/mrobosub_localization/mrobosub_localization/localization.py index 819e3a43..94614e98 100644 --- a/mrobosub_localization/mrobosub_localization/localization.py +++ b/mrobosub_localization/mrobosub_localization/localization.py @@ -1,20 +1,33 @@ +from dataclasses import dataclass import math import numpy as np import rclpy from std_msgs.msg import Float64, Float32 -from mrobosub_lib import Node +from mrobosub_lib.mrobosub_lib import Node from typing import Optional, Final from std_srvs.srv import Trigger -from geometry_msgs.msg import Quaternion -from mrobosub_msgs.msg import ImuINS +from mrobosub_msgs.msg import Imu +from geometry_msgs.msg import Quaternion as ROSQuaternion from math import degrees +@dataclass +class Quaternion: + x: float + y: float + z: float + w: float -def euler_from_quaternion(quaternion): +@dataclass +class Euler: + roll: float + pitch: float + yaw: float + +def euler_from_quaternion(quaternion: Quaternion) -> Euler: """ Converts quaternion (w in last place) to euler roll, pitch, yaw quaternion = [w, x, y, z] @@ -36,10 +49,10 @@ def euler_from_quaternion(quaternion): cosy_cosp = 1 - 2 * (y * y + z * z) yaw = np.arctan2(siny_cosp, cosy_cosp) - return roll, pitch, yaw + return Euler(roll, pitch, yaw) -def quaternion_from_euler(roll, pitch, yaw): +def quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Quaternion: """ Converts euler roll, pitch, yaw to quaternion (w in last place) quat = [x, y, z, w] @@ -52,11 +65,11 @@ def quaternion_from_euler(roll, pitch, yaw): cr = math.cos(roll * 0.5) sr = math.sin(roll * 0.5) - q = [0.0] * 4 - q[0] = cy * cp * cr + sy * sp * sr - q[1] = cy * cp * sr - sy * sp * cr - q[2] = sy * cp * sr + cy * sp * cr - q[3] = sy * cp * cr - cy * sp * sr + q = Quaternion(0.0, 0.0, 0.0, 0.0) + q.x = cy * cp * cr + sy * sp * sr + q.y = cy * cp * sr - sy * sp * cr + q.z = sy * cp * sr + cy * sp * cr + q.w = sy * cp * cr - cy * sp * sr return q @@ -65,7 +78,7 @@ class StateEstimation(Node): """ Subscribers - /depth/raw_depth - - /imu_INS + - /imu """ """ @@ -93,7 +106,7 @@ def __init__(self): self.create_subscription( Float32, "/depth/raw_depth", self.raw_depth_callback, qos_profile=1 ) - self.create_subscription(ImuINS, "/imu_INS", self.imu_callback, qos_profile=1) + self.create_subscription(Imu, "/imu", self.imu_callback, qos_profile=1) self.create_service(Trigger, "localization/zero_state", self.handle_reset) def handle_reset( @@ -121,32 +134,28 @@ def raw_depth_callback(self, raw_depth: Float32): self.heave_offset = raw_depth.data self.heave_pub.publish(Float64(data=raw_depth.data - self.heave_offset)) - def imu_callback(self, msg: ImuINS): - - # orientation = msg.orientation - # - # quaternion = [ - # orientation.x, - # orientation.y, - # orientation.z, - # orientation.w - # ] - # euler = euler_from_quaternion(quaternion) - - euler = msg.theta + def imu_callback(self, msg: Imu): + orientation = msg.orientation + quaternion = Quaternion( + orientation.x, + orientation.y, + orientation.z, + orientation.w + ) + euler = euler_from_quaternion(quaternion) if ( self.yaw_offset is None or self.pitch_offset is None or self.roll_offset is None ): - self.yaw_offset = degrees(-euler.z) - self.pitch_offset = degrees(-euler.y) - self.roll_offset = degrees(euler.x) + self.yaw_offset = degrees(-euler.yaw) + self.pitch_offset = degrees(-euler.pitch) + self.roll_offset = degrees(euler.roll) - yaw = degrees(-euler.z) - self.yaw_offset - pitch = degrees(-euler.y) - self.pitch_offset - roll = degrees(euler.x) - self.roll_offset + yaw = degrees(-euler.yaw) - self.yaw_offset + pitch = degrees(-euler.pitch) - self.pitch_offset + roll = degrees(euler.roll) - self.roll_offset self.yaw_pub.publish(Float64(data=yaw)) self.pitch_pub.publish(Float64(data=pitch)) diff --git a/mrobosub_msgs/msg/Imu.msg b/mrobosub_msgs/msg/Imu.msg index 85fb8715..18722567 100644 --- a/mrobosub_msgs/msg/Imu.msg +++ b/mrobosub_msgs/msg/Imu.msg @@ -1,6 +1,6 @@ std_msgs/Header header geometry_msgs/Vector3 angular_velocity geometry_msgs/Vector3 linear_acceleration -geometry_msgs/Vector3 theta # angle (rodriguez vector) -float64 dt # Time difference since last measurement in seconds - # (Used to calculate velocity and accel from diffs) \ No newline at end of file +geometry_msgs/Quaternion orientation +float64 dt # Time difference since last measurement in seconds + # (Used to calculate velocity and accel from diffs) \ No newline at end of file From e6ddb6b8a0168095965149cbd29ae882bd719550 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Mon, 13 Apr 2026 08:37:28 +0000 Subject: [PATCH 22/31] Finished ROS2 migration of ROS objects --- mrobosub_localization_cpp/CMakeLists.txt | 11 + .../mrobosub_localization_cpp/PosegraphNode.h | 95 ++-- mrobosub_localization_cpp/package.xml | 3 + .../src/PosegraphNode.cpp | 504 ++++++++++++++++++ .../src/localization.cpp | 12 +- 5 files changed, 584 insertions(+), 41 deletions(-) create mode 100644 mrobosub_localization_cpp/src/PosegraphNode.cpp diff --git a/mrobosub_localization_cpp/CMakeLists.txt b/mrobosub_localization_cpp/CMakeLists.txt index 005878b1..f0706f32 100644 --- a/mrobosub_localization_cpp/CMakeLists.txt +++ b/mrobosub_localization_cpp/CMakeLists.txt @@ -1,6 +1,11 @@ cmake_minimum_required(VERSION 3.8) project(mrobosub_localization_cpp) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -8,11 +13,14 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) find_package(Eigen3 REQUIRED) find_package(GTSAM REQUIRED) find_package(BOOST REQUIRED COMPONENTS thread ) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) # Add source files to executable add_executable(localization @@ -23,6 +31,9 @@ add_executable(localization # We can use ament to include+link rclcpp in one line ament_target_dependencies(localization rclcpp + tf2_ros + geometry_msgs + nav_msgs ) target_include_directories(localization PRIVATE diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h index 41ed9fec..279a5036 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h @@ -17,58 +17,62 @@ #include // boost::condition_variable #include // boost::shared_ptr +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + namespace localization { -class PosegraphNode { +class PosegraphNode : public rclcpp::Node { public: PosegraphNode(); - PosegraphNode(std::string config_file); - ~PosegraphNode(); private: // Members - Posegraph *_posegraph; + std::unique_ptr _posegraph; // Define subscribers - int /*ros::Subscriber*/ _imu_sub; - int /*ros::Subscriber*/ _dvl_sub; - int /*ros::Subscriber*/ _baro_sub; - int /*ros::Subscriber*/ _dvl_local_sub; + rclcpp::Subscription::SharedPtr _imu_sub; + rclcpp::Subscription::SharedPtr _dvl_sub; + rclcpp::Subscription::SharedPtr _baro_sub; + rclcpp::Subscription::SharedPtr _dvl_local_sub; // Define publishers - int /*ros::Publisher*/ _pose_pub; - int /*ros::Publisher*/ _dvl_local_pose_pub; - int /*ros::Publisher*/ _path_pub; - int /*nav_msgs::Path*/ _path_msg; - - // TODO: I think these might have to do with the MultiLevelExecutors and stuff like that. - std::unique_ptr _imu_async_spinner; - std::unique_ptr _async_spinner; + rclcpp::Publisher::SharedPtr _pose_pub; + rclcpp::Publisher::SharedPtr _dvl_local_pose_pub; + rclcpp::Publisher::SharedPtr _path_pub; - int /* ros::CallbackQueue */ _imu_queue; + // Define callback groups + rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _imu_callback_group; + rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _low_freq_sensor_callback_group; + rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _posegraph_callback_group; + rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _keyframe_callback_group; - // TODO: Can this be unsigned? - int64_t _frame_count; + rclcpp::TimerBase::SharedPtr _posegraph_timer; + rclcpp::TimerBase::SharedPtr _keyframe_timer; - // Node Handles - // TODO: I don't think these exist for ROS2 - int /* ros::NodeHandle */ _nh; - int /* ros::NodeHandle */ _nh_private; + uint64_t _frame_count; // Barometer state variables - double _first_depth = 0.0; + double _first_depth; // DVL state - gtsam::PreintegratedCombinedMeasurements *_preintegrated_measurements_dvl; - double _prev_dvl_time = 0.0; - double _prev_dvl_local_time = 0.0; - gtsam::Pose3 _prev_dvl_local_pose; - gtsam::Rot3 _dvl_prev_rot; + std::unique_ptr _preintegrated_measurements_dvl; + // First gtsam::Vector3 _first_dvl_vel; + // Previous + double _prev_dvl_time; + double _prev_dvl_local_time; + gtsam::Pose3 _prev_dvl_local_pose; + gtsam::Rot3 _prev_dvl_rot; + // Latest + gtsam::Vector3 _latest_dvl_vel; + gtsam::Pose3 _latest_dvl_pose; // Keyframe states double _first_keyframe_time; - double _prev_keyframe_time = 0.0; - double _current_keyframe_time = 0.0; - bool _is_new_keyframe = false; // Used to be new_kf_flag + double _prev_keyframe_time; + double _current_keyframe_time; + bool _is_new_keyframe; // Used to be new_kf_flag double _keyfram_gap_time; gtsam::Pose3 _latest_keyframe_pose; gtsam::Pose3 _latest_publish_pose; // what does this mean @@ -79,15 +83,20 @@ class PosegraphNode { // IMU State gtsam::Rot3 _imu_latest_rot; gtsam::NavState _latest_imu_prop_state; - int _imu_init_count = 0; // Can this be unsigned? - int _imu_count = 0; // Can this be unsigned? + uint32_t _imu_init_count; + uint32_t _imu_count; + std::vector _imu_init_acc; + std::vector _imu_init_rot; - boost::mutex _mtx; - boost::condition_variable _keyframe_cv; + // Transforms + std::unique_ptr _tf_broadcaster; + + std::mutex _mtx; + std::condition_variable _cond_var; // Get from config file. - bool _is_using_dvl_v2_factor = true; - bool _is_rot_initialized = false; + bool _is_using_dvl_v2_factor; + bool _is_rot_initialized; /* TODO: What is save trajectory?? */ // ros::ServiceServer _save_trajectory_service; @@ -98,6 +107,16 @@ class PosegraphNode { private: // methods /* TODO: What is save trajectory?? */ // bool save_trajectory(turtlmap::save_trajectory::Request &req, turtlmap::save_trajectory::Response &res); + + void imu_callback(const mrobosub_msgs::msg::Imu::SharedPtr msg); + void dvl_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg); + void baro_callback(const std_msgs::msg::Float64::SharedPtr msg); + void dvl_local_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg); + + void main_loop(); + void kf_loop(); + + void broadcast_imu_transform(); }; } // namespace localization diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml index 168bb89d..1d96f816 100644 --- a/mrobosub_localization_cpp/package.xml +++ b/mrobosub_localization_cpp/package.xml @@ -11,6 +11,9 @@ eigen gtsam + geometry_msgs + nav_msgs + tf2_ros gtsam libeigen3-dev ament_lint_auto diff --git a/mrobosub_localization_cpp/src/PosegraphNode.cpp b/mrobosub_localization_cpp/src/PosegraphNode.cpp new file mode 100644 index 00000000..1b4642d1 --- /dev/null +++ b/mrobosub_localization_cpp/src/PosegraphNode.cpp @@ -0,0 +1,504 @@ +#include "PosegraphNode.h" +#include "mrobosub_msgs/msg/Imu.hpp" +#include "mrobosub_msgs/msg/Dvl.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace localization { + +PosegraphNode::PosegraphNode() + : Node("posegraph") + , _first_depth(0.0) + , _prev_dvl_time(0.0) + , _prev_dvl_local_time(0.0) + , _prev_keyframe_time(0.0) + , _current_keyframe_time(0.0) + , _is_new_keyframe(false) + , _imu_init_count(0) + , _imu_count(0) + , _is_using_dvl_v2_factor(true) + , _is_rot_initialized(false) { + + RCLCPP_INFO(this->get_logger(), "'posegraph' node has started"); + + // Create posegraph variables + _posegraph = std::make_unique(); + _preintegrated_measurements_dvl = std::make_unique( + boost::make_shared( + _posegraph->_preintegrated_measurement_params + ), + _posegraph->_prior_imu_bias + ); + _keyframe_gap_time = _posegraph->_pose_graph_params->_keyframe_gap_time; + + // Set up the transform + _tf_broadcaster = std::make_unique(*this); + + // Set up callback groups + _imu_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + _low_freq_sensor_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + _posegraph_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + _keyframe_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + + // Create ROS subscriptions + auto imu_options = rclcpp::SubscriptionOptions(); + imu_options.callback_group = _imu_callback_group; + + auto low_freq_options = rclcpp::SubscriptionOptions(); + low_freq_options.callback_group = _low_freq_callback_group; + + _imu_sub = this->create_subscription( + "/imu", 10, + std::bind(&PosegraphNode::imu_callback, this, std::placeholders::_1), + imu_options + ); + + _dvl_sub = this->create_subscription( + "/dvl", 10, + std::bind(&PosegraphNode::dvl_callback, this, std::placeholders::_1), + low_freq_options + ); + + _baro_sub = this->create_subscription( + "/depth", 10, + std::bind(&PosegraphNode::baro_callback, this, std::placeholders::_1), + low_freq_options + ); + + _dvl_local_sub = this->create_subscription( + "/dvl_local", 10, + std::bind(&PosegraphNode::dvl_local_callback, this, std::placeholders::_1), + low_freq_options + ); + + // Create ROS publishers + _pose_pub = this->create_publisher("/localization/pose", 1000); + _dvl_local_pose_pub = this->create_publisher("/localization/dvl_local_pose", 1000); + _path_pub = this->create_publisher("/localization/path", 1000); + + // Create timers + _posegraph_timer = this->create_wall_timer( + std::chrono::milliseconds(50), + std::bind(&PosegraphNode::main_loop, this), + _posegraph_callback_group + ); + + // TODO: Should this be on a separate keyframe_callback_group? + _keyframe_timer = this->create_wall_timer( + std::chrono::milliseconds(50), + std::bind(&PosegraphNode::kf_loop, this), + _keyframe_callback_group + ); +} + +void PosegraphNode::main_loop() { + if (_is_new_keyframe) { + _is_new_keyframe = false; + _posegraph->add_barometric_factor(_posegraph->get_depth_measurement(), 0.1, _posegraph->_index); + if (_is_using_dvl_v2_factor) { + _posegraph->add_dvl_factor(true); // using slerp true + } else { + throw std::runtime_error("Not implemented yet!"); + } + + if (_posegraph->_index > 0) { + _posegraph->optimize_pose_graph(); + _latest_keyframe_pose = _posegraph->_result->at(gtsam::Symbol('x', _posegraph->_index)); + _latest_publish_pose = _latest_keyframe_pose; + _posegraph->_prev_pose = _latest_keyframe_pose; + if (_is_using_dvl_v2_factor) { + _posegraph->_prior_dvl_bias = _posegraph->_result->at(gtsam::Symbol('d', _posegraph->_index)); + _posegraph->_preintegrated_velocity_measurements->reset_integration_and_bias(_posegraph->_prior_dvl_bias); + } else { + throw std::runtime_error("Not implemented yet!"); + } + } + + _posegraph->_prev_keyframe_time = _current_keyframe_time; + _posegraph->_index++; + _posegraph->_initial->insert(gtsam::('d', _posegraph->_index), _posegraph->_prior_dvl_bias); + } +} + +void PosegraphNode::kf_loop() { + _posegraph->_index++; + + _keyframe_timestamps.push_back(_current_keyframe_time); + double time = _current_keyframe_time - _first_keyframe_time; + _posegraph->_initial->insert(gtsam::Symbol('d', _posegraph->_index), _posegraph->_prior_dvl_bias); + _posegraph->_initial->insert(gtsam::Symbol('b', _posegraph->_index), _posegraph->_prior_imu_bias); + + // Add smoothing timestamps + _posegraph->_smoother_timestamps[gtsam::Symbol('d', _posegraph->_index)] = time; + _posegraph->_smoother_timestamps[gtsam::Symbol('b', _posegraph->_index)] = time; + _posegraph->_smoother_timestamps[gtsam::Symbol('x', _posegraph->_index)] = time; + + // Start to process new kf + if (_posegraph->_index < 10000) { + _posegraph->add_barometric_factor(_posegraph->get_depth_measurement(), 0.005, _posegraph->_index); + } + + if (_posegraph->_index > 1) { + _posegraph->_initial->insert( + gtsam::Symbol('v', _posegraph->_index), + _posegraph->_result->at( + gtsam::Symbol('v', _posegraph->_index-1), + ) + ); + } else { + _posegraph->_initial->insert( + gtsam::Symbol('v', _posegraph->_index), + gtsam::Vector3(0, 0, 0) + ); + } + _posegraph->_smoother_timestamps[gtsam::Symbol('v', _posegraph->_index)] = time; + + _posegraph->add_imu_factor(); + if (_is_using_dvl_v2_factor) { + _posegraph->add_dvl_factor_imu_rotation(); + } else { + throw std::runtime_error("Not implemented yet!"); + } + + if (_posegraph->_index > 0) { + if (!_posegraph->_pose_graph_params->_using_smoother) { + _posegraph->optimize_pose_graph(); + } else { + _posegraph->optimize_pose_graph_smoother(); + } + + _posegraph->_prior_imu_bias = _posegraph->_result->at(gtsam::Symbol('b', _posegraph->_index)); + _posegraph->_preintegrated_measurements->resetIntegrationAndSetBias(_posegraph->_prior_imu_bias); + _preintegrated_measurements_dvl->resetIntegrationAndSetBias(_posegraph->_prior_imu_bias); + + _latest_keyframe_pose = _posegraph->_result->at(gtsam::Symbol('x', _posegraph->_index)); + _latest_publish_pose = _latest_keyframe_pose; + _latest_dvl_pose = _latest_keyframe_pose; + _latest_dvl_vel = _posegraph->_result->at(gtsam::Symbol('v', _posegraph->_index)); + _posegraph->_prev_pose = _latest_keyframe_pose; + + if (_is_using_dvl_v2_factor) { + _posegraph->_prior_dvl_bias = _posegraph->_result->at(gtsam::Symbol('d', _posegraph->_index)) + gtsam::Point3 dvl_bias_point3 = _posegraph->_prior_dvl_bias.accelerometer(); + dvl_bias_point3 = _posegraph->_prev_pose.rotation() * dvl_bias_point3; + _posegraph->_preintegrated_velocity_measurements->reset_integration_and_bias(); + } else { + throw std::runtime_error("Not implemented yet"); + } + } + + _posegraph->_prev_keyframe_time = _current_keyframe_time; + _is_new_keyframe = false; +} + +void PosegraphNode::imu_callback(const mrobosub_msgs::msg::Imu::SharedPtr msg) { + // This function is guaranteed to be mutually exclusive to all other + // functions since it is on a separate timer. + + if (!_is_rot_initialized) { + _imu_init_count++; + // get the rotation from the first imu message + gtsam::Rot3 imu_rot = gtsam::Rot3(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z); + _imu_init_rot.push_back(imu_rot.xyz()); + } else { + gtsam::Vector3 imu_acc = gtsam::Vector3(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z); + gtsam::Vector3 imu_gyro = gtsam::Vector3(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); + _imu_latest_rot = gtsam::Rot3(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z); + _posegraph->_preintegrated_measurements->integrateMeasurement(imu_acc, imu_gyro, _posegraph->_pose_graph_params->_imu_params.dt_imu); + _preintegrated_measurements_dvl->integrateMeasurement(imu_acc, imu_gyro, _posegraph->_pose_graph_params->_imu_params.dt_imu); + _imu_count++; + + if (_imu_count >= 5) { + _latest_imu_prop_state = _preintegrated_measurements_dvl->predict(gtsam::NavState( + _latest_dvl_pose, + _latest_dvl_pose.rotation() * _latest_dvl_vel, + _posegraph->_prior_imu_bias + )) + + gtsam::Pose3 latest_pose = _latest_imu_prop_state.pose();; + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = imu_msg->header.stamp; + pose_msg.header.frame_id = "NED_imu"; + pose_msg.pose.position.x = latest_pose.translation().x(); + pose_msg.pose.position.y = latest_pose.translation().y(); + pose_msg.pose.position.z = latest_pose.translation().z(); + pose_msg.pose.orientation.w = latest_pose.rotation().toQuaternion().w(); + pose_msg.pose.orientation.x = latest_pose.rotation().toQuaternion().x(); + pose_msg.pose.orientation.y = latest_pose.rotation().toQuaternion().y(); + pose_msg.pose.orientation.z = latest_pose.rotation().toQuaternion().z(); + _pose_pub.publish(pose_msg); + + // Transform + broadcast_imu_transform(); + _imu_count = 0; + } + } +} + +void PosegraphNode::dvl_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg) { + if(!_is_rot_initialized) return; + + const std::unique_lock lock(_mtx); + double dt_dvl; + double dvl_current_time = msg->header.stamp.toSec(); + gtsam::Vector3 dvl_vel = _posegraph->_T_SD.block(0, 0, 3, 3) * + gtsam::Vector3(msg->velocity.x, msg->velocity.y, msg->velocity.z); + double fom = msg->fom; + bool is_valid = msg->velocity_valid; + + // Handle initial measurement + if (_prev_dvl_time == 0.0) { + _prev_dvl_time = dvl_current_time; + dt_dvl = dvl_current_time - _current_keyframe_time; + _prev_dvl_rot = _imu_latest_rot; + _first_dvl_vel = dvl_vel; + + _posegraph->_initial->insert( + gtsam::Symbol('v', _posegraph->_index), + _first_dvl_vel + ); + _posegraph->_graph->add(gtsam::PriorFactor( + gtsam::Symbol('v', 0), + _first_dvl_vel, + gtsam::noiseModel::Diagonal::shared_ptr prior_vel_noise = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(3) << 0.1, 0.1, 0.1).finished() + ) + )) + + // Update for preintegrated_measurements_dvl + _latest_dvl_vel = _first_dvl_vel; + _latest_dvl_pose = _latest_publish_pose; + } else { + dt_dvl = dvl_current_time - _prev_dvl_time; + } + + if (fom >= _posegraph->_pose_graph_params->_dvl_params.fom_threshold && !is_valid) { + gtsam::NavState imu_prop_state = _preintegrated_measurements_dvl->predict( + gtsam::NavState(_latest_dvl_pose), + _latest_dvl_pose.rotation() * _latest_dvl_vel, + _posegraph->_prior_imu_bias + ) + + gtsam::Vector3 old_dvl_vel = dvl_vel; + dvl_vel = imu_prop_state.pose().rotation().inverse() * imu_prop_state.velocity(); + + // Compute difference vector + gtsam::Vector3 dvl_vel_diff = dvl_vel - old_dvl_vel; + + RCLCPP_WARN(this->get_logger(), "Invalid DVL Measurement with diff", dvl_vel_diff); + } + + // Communicate current dvl to posegraph + _posegraph->add_dvl_velocity(dvl_current_time, dvl_vel); + + gtsam::Rot3 d_rot = _prev_dvl_rot.inverse() * _imu_latest_rot; + gtsam::Point3 d_position = d_rot.matrix() * gtsam::Point3( + dvl_vel.x() * dt_dvl, + dvl_vel.y() * dt_dvl, + dvl_vel.z() * dt_dvl, + ); + gtsam::Pose3 d_pose(d_rot, d_position); + _latest_publish_pose = _latest_publish_pose * d_pose; + + // Update latest + _latest_dvl_vel = dvl_vel; + _latest_dvl_pose = _latest_publish_pose; + _preintegrated_measurements_dvl->resetIntegration(); + + _posegraph->_curr_dvl_foms.push_back(fom * 4); // TDOO: multiply fom by 4 to get actual? + _posegraph->_imu_rot_list.push_back(_imu_latest_rot); + _prev_dvl_time = dvl_current_time; + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = msg->header.stamp; + pose_msg.header.frome_id = "NED_imu"; + pose_msg.pose.position.x = _latest_publish_pose.translation().x(); + pose_msg.pose.position.y = _latest_publish_pose.translation().y(); + pose_msg.pose.position.z = _latest_publish_pose.translation().z(); + pose_msg.pose.orientation.w = _latest_publish_pose.rotation().toQuaternion().w(); + pose_msg.pose.orientation.x = _latest_publish_pose.rotation().toQuaternion().x(); + pose_msg.pose.orientation.y = _latest_publish_pose.rotation().toQuaternion().y(); + pose_msg.pose.orientation.z = _latest_publish_pose.rotation().toQuaternion().z(); + _pose_pub.publish(pose_msg); + + broadcast_imu_transform(); + + geometry_msgs::msg::TransformStamped world_tf_msg; + world_tf_msg.header.stamp = msg->header.stamp; + world_tf_msg.header.frame_id = "world"; + world_tf_msg.child_frame_id = "NED_imu"; + world_tf_msg.transform.translation.x = 0.0; + world_tf_msg.transform.translation.y = 0.0; + world_tf_msg.transform.translation.z = 0.0; + world_tf_msg.transform.rotation.w = 0.0; + world_tf_msg.transform.rotation.x = 1.0; + world_tf_msg.transform.rotation.y = 0.0; + world_tf_msg.transform.rotation.z = 0.0; + br.sendTransform(world_tf_msg); + + _prev_dvl_rot = _imu_latest_rot; +} + +void PosegraphNode::baro_callback() { + std::unique_lock lock(_mtx); + if (!_is_rot_initialized) { + _cond_var.wait(lock, [_imu_init_rot] { return _imu_init_rot.size() < 5; }); + gtsam::Vector3 imu_rot_mean(0.0, 0.0, 0.0); + int latest_imu_rot_number = 5; + for (int i = _imu_init_rot.size() - latest_imu_rot_number; i < _imu_init_rot.size(); ++i) { + imu_init_mean += _imu_init_rot[i]; + } + imu_rot_mean /= latest_imu_rot_number; + + gtsam::Rot3 imu_rot = gtsam::Rot3::RzRyRx(imu_rot_mean[0], imu_rot_mean[1], imu_rot_mean[2]); + _imu_latest_rot = imu_rot; + _posegraph->_imu_prev_rot = imu_rot; + _imu_init_rot.clear(); + + _posegraph->initialize_pose_graph_from_imu(imu_rot); + _current_keyframe_time = msg->header.stamp.toSec(); + if (_posegraph->_pose_graph_params->_using_smoother) { + _posegraph->_smoother_timestamps[gtsam::Symbol('x', _posegraph->_index)] = 0.0; + _posegraph->_smoother_timestamps[gtsam::Symbol('v', _posegraph->_index)] = 0.0; + _posegraph->_smoother_timestamps[gtsam::Symbol('b', _posegraph->_index)] = 0.0; + _posegraph->_smoother_timestamps[gtsam::Symbol('d', _posegraph->_index)] = 0.0; + } + + _first_keyframe_time = _current_keyframe_time; + _posegraph->_prev_keyframe_time = _current_keyframe_time; + _keyframe_timestamps.push_back(_current_keyframe_time); + _latest_keyframe_pose = _posegraph->_initial->at(gtsam::Symbol('x', 0)); + _latest_publish_pose = _posegraph->_initial->at(gtsam::Symbol('x', 0)); + + _is_rot_initialized = true; + + // TODO: what is this calculation??? + double depth = (msg->fluid_pressure - _posegraph->_pose_graph_params->_barometer_params.atmospheric_pressure) * 100 / 9.81 * 997.0; + _first_depth = depth; + return; + } + + double depth = (msg->fluid_pressure - _posegraph->_pose_graph_params->_barometer_params.atmospheric_pressure) * 100 / 9.81 * 997.0; + if (_first_depth == 0.0) { + _first_depth = depth; + } + _posegraph->set_depth_measurement(depth - _first_depth); + + double baro_current_time = msg->header.stamp.toSec(); + if (baro_current_time - _current_keyframe_time > _keyframe_gap_time) { + if (_posegraph->_pose_graph_params->_using_pseudo_dvl) { + gtsam::NavState pseudo_state = _preintegrated_measurements_dvl->predict( + gtsam::NavState( + _latest_dvl_pose, + _latest_dvl_pose.rotation() * _latest_dvl_vel + ), + _posegraph->_prior_imu_bias + ); + + gtsam::Vector3 pseudo_dvl_vel = pseudo_state.pose().rotation().inverse() * pseudo_state.velocity(); + + _posegraph->add_dvl_velocity(baro_current_time, pseudo_dvl_vel); + _posegraph->_imu_rot_list.push_back(_imu_latest_rot); + _posegraph->_curr_dvl_foms.push_back(0.02); + _prev_dvl_time = baro_current_time; + } + _current_keyframe_time = baro_current_time; + _new_keyframe_flag = true; + _cond_var.notify_one(); + } +} + +void PosegraphNode::dvl_local_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg) { + std::unique_lock lock(_mtx); + double dt_dvl_local; + double dvl_local_current_time = msg->header.stamp.toSec(); + gtsam::Pose3 dvl_local_pose = gtsam::Pose3( + gtsam::Rot3( + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z + ), + gtsam::Point3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z + ) + ); + + if (_prev_dvl_local_time == 0.0) { + _prev_dvl_local_time = dvl_local_current_time; + dt_dvl_local = dvl_local_current_time - _current_keyframe_time; + _prev_dvl_local_pose = dvl_local_pose; + _T_w_wd = _latest_publish_pose * gtsam::Pose3(_posegraph->_T_SD) * _prev_dvl_local_pose.inverse(); + } else { + dt_dvl_local = dvl_local_current_time - _prev_dvl_local_time; + } + + Eigen::Quaterniond q( + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z + ); + gtsam:Rot3 dvl_pose = gtsam::Pose3( + gtsam::Rot3(q.normalized().toRotationMatrix()) + gtsam::Point3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z + ) + ); + + gtsam::Pose3 d_pose = _prev_dvl_local_pose.inverse() * dvl_pose; + _posegraph->add_dvl_pose(dvl_local_current_time, d_pose); + dvl_local_pose = _T_w_wd * _dvl_local_pose * gtsam::Pose3(_posegraph->_T_SD).inverse(); + + gtsam::Pose3 T_sensor_zed = gtsam::Pose3( + gtsam::Rot3(0, 0, 0, 0), + gtsam::Point3(0, 0, 0) + ) + dvl_local_pose = dvl_local_pose * T_sensor_zed.inverse(); + + geometry_msgs::msg::TransformStamped dvl_local_pose_msg; + dvl_local_pose_msg.header.stamp = msg->header.stamp; + dvl_local_pose_msg.header.frame_id = "NED_imu"; + dvl_local_pose_msg.pose.position.x = dvl_local_pose.translation().x(); + dvl_local_pose_msg.pose.position.y = dvl_local_pose.translation().y(); + dvl_local_pose_msg.pose.position.z = dvl_local_pose.translation().z(); + dvl_local_pose_msg.pose.orientation.w = dvl_local_pose.rotation().toQuaternion().w(); + dvl_local_pose_msg.pose.orientation.x = dvl_local_pose.rotation().toQuaternion().x(); + dvl_local_pose_msg.pose.orientation.y = dvl_local_pose.rotation().toQuaternion().y(); + dvl_local_pose_msg.pose.orientation.z = dvl_local_pose.rotation().toQuaternion().z(); + _dvl_local_pose_publisher.publish(dvl_local_pose_msg); + + _prev_dvl_local_time = dvl_local_current_time; + _prev_dvl_local_pose = dvl_pose; +} + +void PosegraphNode::broadcast_imu_transform() { + // Back calculate where the base_link must be based off of where the sensor says it is. + // T_sensor_zed is the static offset of the sensor relative to the robot's origin + // TODO! + gtsam::Pose3 T_sensor_zed = gtsam::Pose3( + gtsam::Rot3(0, 0, 0, 0), + gtsam::Point3(0, 0, 0) + ) + gtsam::Pose3 latest_publish_pose_base_link = latest_pose * T_sensor_zed.inverse(); + + geometry_msgs::msg::TransformStamped pose_tf_msg; + pose_tf_msg.header.stamp = imu_msg->header.stamp; + pose_tf_msg.header.frame_id = "NED_imu"; + pose_tf_msg.child_frame_id = "base_link"; + pose_tf_msg.pose.position.x = latest_pose.translation().x(); + pose_tf_msg.pose.position.y = latest_pose.translation().y(); + pose_tf_msg.pose.position.z = latest_pose.translation().z(); + pose_tf_msg.pose.orientation.w = latest_pose.rotation().toQuaternion().w(); + pose_tf_msg.pose.orientation.x = latest_pose.rotation().toQuaternion().x(); + pose_tf_msg.pose.orientation.y = latest_pose.rotation().toQuaternion().y(); + pose_tf_msg.pose.orientation.z = latest_pose.rotation().toQuaternion().z(); + _tf_broadcaster.sendTransform(pose_tf_msg); +} + + + +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/localization.cpp b/mrobosub_localization_cpp/src/localization.cpp index 7c38a519..5da1e625 100644 --- a/mrobosub_localization_cpp/src/localization.cpp +++ b/mrobosub_localization_cpp/src/localization.cpp @@ -1,10 +1,16 @@ #include +#include "PosegraphNode.h" int main(int argc, char ** argv) { - (void) argc; - (void) argv; + rclcpp::init(argc, argv); + auto node = std::make_shared(); - printf("hello world mrobosub_localization_cpp package\n"); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + + executor.spin(); + + rclcpp::shutdown(); return 0; } From 860d19323aef0fe8c8120fe12c9325c53ab39cce Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 14 Apr 2026 20:23:26 +0000 Subject: [PATCH 23/31] Changes sensor publishers to use standard ROS2 types --- mrobosub_hal/mrobosub_hal/arduino.py | 7 +- mrobosub_hal/mrobosub_hal/dvl_publisher.py | 117 ++++++++++++++++++--- mrobosub_hal_imu/src/imu_node.cpp | 38 +++++-- mrobosub_localization_cpp/CMakeLists.txt | 1 + mrobosub_localization_cpp/README.md | 27 +++++ mrobosub_localization_cpp/package.xml | 1 + mrobosub_msgs/msg/Dvl.msg | 4 +- 7 files changed, 168 insertions(+), 27 deletions(-) diff --git a/mrobosub_hal/mrobosub_hal/arduino.py b/mrobosub_hal/mrobosub_hal/arduino.py index ad528cf2..a9a83685 100755 --- a/mrobosub_hal/mrobosub_hal/arduino.py +++ b/mrobosub_hal/mrobosub_hal/arduino.py @@ -1,10 +1,11 @@ import rclpy from serial import Serial from std_msgs.msg import Float32, Bool +from sensor_msgs.msg import FluidPressure import time import struct -from mrobosub_lib import Node +from mrobosub_lib.mrobosub_lib import Node FREQUENCY = 60 # times per second BAUD_RATE = 9600 @@ -21,7 +22,7 @@ def __init__(self): Bool, "/buttons/strange", qos_profile=1 ) self.depth_pub = self.create_publisher( - Float32, "/depth", qos_profile=1 + FluidPressure, "/depth", qos_profile=1 ) #self.write_timer = self.create_timer(1/FREQUENCY, self.writer) @@ -64,7 +65,7 @@ def serialConnection(self): self.charm_pub.publish(Bool(data=(self.charm == b'\x01'))) self.strange_pub.publish(Bool(data=(self.strange == b'\x01'))) - self.depth_pub.publish(Float32(data=self.depth)) + self.depth_pub.publish(FluidPressure(fluid_pressure=self.depth)) def main(): diff --git a/mrobosub_hal/mrobosub_hal/dvl_publisher.py b/mrobosub_hal/mrobosub_hal/dvl_publisher.py index 38456027..b68a576e 100755 --- a/mrobosub_hal/mrobosub_hal/dvl_publisher.py +++ b/mrobosub_hal/mrobosub_hal/dvl_publisher.py @@ -1,16 +1,16 @@ import socket import rclpy -from mrobosub_msgs.msg import Dvl import numpy as np -from mrobosub_lib import Node - +from mrobosub_lib.mrobosub_lib import Node +from geometry_msgs.msg import TwistWithCovarianceStamped +from mrobosub_msgs.msg import Dvl -# todo: parameterize this in the launch file -UDP_IP = "0.0.0.0" -# UDP_IP = "192.168.2.9" +UDP_IP = b"192.168.2.9" HOST_IP = b"192.168.2.3" UDP_PORT = 27000 +POS_UP = True +ALPHA = 20 # deg from vertical class DVLPublisher (Node): """ @@ -19,7 +19,10 @@ class DVLPublisher (Node): def __init__(self): super().__init__('dvl_publisher') - self.pub = self.create_publisher(Dvl, "/dvl/raw_dvl", qos_profile=1) + + self.twist_pub = self.create_publisher(TwistWithCovarianceStamped, "/dvl/twist", qos_profile=1) + self.local_pose_pub = self.create_publisher(Dvl, "/dvl/local", qos_profile=1) + self.T_beams_xyz = self.construct_transformation_matrix() # connect to socket containing the DVL information self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) @@ -42,16 +45,104 @@ def loop(self): data_str = data.decode() if not data_str.startswith("$DVKFC"): - return - - # parse according to spec here https://docs.ceruleansonar.com/c/dvl-50/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvkfc-kalman-filter-raw-data-support-message - data_list = data_str.split(",") - map_list = [*map(float, data_list[10 : 24 + 1 : 7])] - self.pub.publish(Dvl(velocity=map_list)) + self.publish_twist(data) + elif not data_str.startswith("$DVKFC"): + self.publish_local_pose(data) + + self.publish_twist(data) except socket.timeout: self.get_logger().info("DVL UDP connection timing out, no data recieved from DVL") + def publish_twist(self, data): + # parse according to spec here https://docs.ceruleansonar.com/c/dvl-50/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvkfc-kalman-filter-raw-data-support-message + data_list = data.split(b",") + va, vb, vc = map(float, data_list[10 : 25 : 7]) + + # The DVL provides confidence values in each of the velocity axes + # Take the biggest (worst) confidence value as variance. + # Technically, values greater than 0.5 should be chucked out. + va_c, vb_c, vc_c = map(float, data_list[11 : 26 : 7]) + max_var = max(va_c, vb_c, vc_c) + if max_var > 0.5: + return + + vx, vy, vz = self.calculate_robot_velocity_from_beams(va, vb, vc) + + # Create the message + msg = TwistWithCovarianceStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "dvl_link" + + # Write in the velocities + msg.twist.twist.linear.x = vx + msg.twist.twist.linear.y = vy + msg.twist.twist.linear.z = vz + + if POS_UP: + msg.twist.twist.linear.z *= 1 # Flip from down-positive to up-positive + + # The covariance matrix is a flattened 6x6 matrix. We only need to place the 0th, 7th, and 14th values. + cov = [0.0] * 36 + cov[0] = max_var + cov[7] = max_var + cov[14] = max_var + msg.twist.covariance = cov + + # Publish the msg + self.twist_pub.publish(msg) + + def publish_local_pose(self, data): + # parse according to spec here https://docs.ceruleansonar.com/c/tracker-650/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvpdl-and-usddvpdx-dvl-position-and-angle-deltas-messages + data_list = data.split(b",") + # tu=1, dtu=2, adr=3, adp=4, ady=5, pdx=6, pdy=7, pdz=8, c=9 + try: + confidence = float(data_list[9]) + if confidence <= 0: return # don't trust data with negative confidence + + local_msg = Dvl() + local_msg.header.stamp = self.get_clock().now().to_msg() + + local_msg.ad[0] = float(data_list[3]) # roll + local_msg.ad[1] = float(data_list[4]) # pitch + local_msg.ad[2] = float(data_list[5]) # yaw + + local_msg.pd[0] = float(data_list[6]) # x + local_msg.pd[1] = float(data_list[7]) # y + local_msg.pd[2] = float(data_list[8]) # z + + if POS_UP: + local_msg.pd[2] *= -1 # Flip from down-positive to up-positive + + local_msg.confidence = confidence + + self.local_pose_pub.publish(local_msg) + except (IndexError, ValueError): + pass + + def calculate_robot_velocity_from_beams(self, va, vb, vc): + ''' + Returns the velocity of the robot in the Forward-Left-Down frame from the respective beam velocities + ''' + beams = np.array([va, vb, vc]) + v_robot = self.T_beams_xyz @ beams + return v_robot + + def construct_transformation_matrix(self): + # Since the Tracker 650 is a 3-beam DVL, this means that each beam is aligned at a 120deg offset + # from each other. Additionally, there is a 70def beam angle from the horizontal + # We need to apply a transformation matrix to the beam measurements to recover the robot frame velocity + alpha = np.radians(ALPHA) + + s_a = np.sin(alpha) + c_a = np.cos(alpha) + + self.T_beam_xyz = np.array([ + [(2/3)/s_a, (-1/3)/s_a, (-1/3)/s_a], # Vx (Forward) + [ 0, (1/np.sqrt(3))/s_a, (-1/np.sqrt(3))/s_a], # Vy (Left) + [(1/3)/c_a, (1/3)/c_a, (1/3)/c_a], # Vz (Down-positive) + ]) + def main(): rclpy.init() node = DVLPublisher() diff --git a/mrobosub_hal_imu/src/imu_node.cpp b/mrobosub_hal_imu/src/imu_node.cpp index 6ee92c74..e8533a4d 100644 --- a/mrobosub_hal_imu/src/imu_node.cpp +++ b/mrobosub_hal_imu/src/imu_node.cpp @@ -6,7 +6,8 @@ #include "../inertial-sense-sdk/src/data_sets.h" #include -#include +// #include +#include #include #include @@ -66,7 +67,7 @@ int main(int argc, char **argv) return 1; } - auto imu_pub = node->create_publisher("/imu", 1); + auto imu_pub = node->create_publisher("/imu", 1); if (!imu_pub) { return -1; } @@ -97,19 +98,34 @@ int main(int argc, char **argv) auto time_stamp = timeManager.ros_time_from_start_time(ins->timeOfWeek); const auto div = 1.0f/data->dt; - mrobosub_msgs::msg::Imu msg; - msg.header.stamp = time_stamp; - msg.dt = data->dt; + sensor_msgs::msg::Imu msg; + msg.header.stamp = time_stamp; + msg.header.frame_id = "imu_link"; + + msg.orientation.w = data->qn2b[0]; + msg.orientation.x = data->qn2b[1]; + msg.orientation.y = data->qn2b[2]; + msg.orientation.z = data->qn2b[3]; + msg.angular_velocity.x = pimu_storage.pimu.theta[0] * div; msg.angular_velocity.y = pimu_storage.pimu.theta[1] * div; msg.angular_velocity.z = pimu_storage.pimu.theta[2] * div; + msg.linear_acceleration.x = pimu_storage.pimu.vel[0] * div; msg.linear_acceleration.y = pimu_storage.pimu.vel[1] * div; msg.linear_acceleration.z = pimu_storage.pimu.vel[2] * div; - msg.theta.w = data->theta[0]; - msg.theta.x = data->theta[1]; - msg.theta.y = data->theta[2]; - msg.theta.z = data->theta[3]; + + double acc_var = 3.45e-7; // Acc noise density is 0.000588 -> var is noise^2 + double gyro_var = 7.61e-9; // Gyro noise density is 8.72e-5 -> var is noise^2 + + msg.linear_acceleration_covariance[0] = acc_var; + msg.linear_acceleration_covariance[4] = acc_var; + msg.linear_acceleration_covariance[8] = acc_var; + + msg.angular_acceleration_covariance[0] = gyro_var; + msg.angular_acceleration_covariance[4] = gyro_var; + msg.angular_acceleration_covariance[8] = gyro_var; + imu_pub->publish(msg); } pimu_storage.has_pimu = false; @@ -148,6 +164,8 @@ int main(int argc, char **argv) return 1; } rclcpp::spin_some(node); - std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + // Try to get the publishing rate closer to the rate ascertained by the datasheet + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } diff --git a/mrobosub_localization_cpp/CMakeLists.txt b/mrobosub_localization_cpp/CMakeLists.txt index f0706f32..bcc52126 100644 --- a/mrobosub_localization_cpp/CMakeLists.txt +++ b/mrobosub_localization_cpp/CMakeLists.txt @@ -33,6 +33,7 @@ ament_target_dependencies(localization rclcpp tf2_ros geometry_msgs + sensor_msgs nav_msgs ) diff --git a/mrobosub_localization_cpp/README.md b/mrobosub_localization_cpp/README.md index 3b2e5d36..833125eb 100644 --- a/mrobosub_localization_cpp/README.md +++ b/mrobosub_localization_cpp/README.md @@ -135,6 +135,33 @@ In order to access a node's parameters, you have to have a pointer to the node i If you want to access a shared_ptr to `this` you can use `this->shared_from_this`. However, you can't pass a `shared_ptr` of a class into any other class or for any other use-case (as it does not exist!!!) until **AFTER THE CONSTRUCTOR RUNS**. I did not know this! As such, you must have a separate `initialize_parameters` method that you will run to populate the parameters after +### [4/13] Determining Parameter Values +To determine the various parameter values, I started off with the datasheets for each of the sensors. + +#### Inertial Sense IMX-5 +The Inertial Sense IMX-5 (IMU)'s datasheet is [here](https://docs.inertialsense.com/datasheets/IMX-5_IMU_AHRS_GNSS-INS_Datasheet.pdf). From the datasheet, I was able to get the arr/gyro maximum values, noise density, and random walk values. + +According to the datasheet, apparently I am able to get readings at a rate of 1kHz which is really good (4x the rate that the TURTLMap IMU was able to get). + +Since I am having this node run in it's own package, I might be able to bump up the ROS publishing rate. Right now I am having the node sleep for 5ms before continuing --> this maxes out my maximum rate to 200kHz. + +I also changed the message type from our custom type to the `sensor_msgs::msg::Imu` type to be more in line with the traditional ROS2 sensor types. + +### Tracker 650 DVL +I primarily just need the extrinsic transformations from the CAD. Will ask Mechanical for those numbers ASAP. + +I needed to change the data published by the DVL because according to the datasheet [here](https://docs.ceruleansonar.com/c/tracker-650/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvkfc-kalman-filter-raw-data-support-message) the DVL actually publishes it's confidence in each of it's axes. This can be used to provide a covariance value. + +Apparently the typical ROS method for publishing messages parsed from a DVL is the `TwistWithCovarianceStamped` (which makes sense since the DVL provides the sub an estimate of it's twist). + +Right now, we were extracting the raw beam velocities in the DVL publisher node as opposed to publishing the vx, vy, vz. I had to use a transformation matrix to turn beam velocities into robot frame velocities. + +The sensor is mounted via a NED format. Using right hand rule, point your index finger away from the data cable, your middle finger towards the right, and your thumb pointed down. + +See the following reference image. +![image](https://docs.ceruleansonar.com/c/~gitbook/image?url=https%3A%2F%2F977450193-files.gitbook.io%2F%7E%2Ffiles%2Fv0%2Fb%2Fgitbook-x-prod.appspot.com%2Fo%2Fspaces%252FldEroQctKFErSiZvuhJk%252Fuploads%252FxuL7u4W2BE68qUs2ovN7%252Fimage.png%3Falt%3Dmedia%26token%3D669440b3-49cd-4d2f-a5d5-f8bcedde80e0&width=768&dpr=3&quality=100&sign=d4de0e74&sv=2) + +Note that because of the beam orientations, the A beam is aligned with the x direction. ## Citation diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml index 1d96f816..9a83c8d9 100644 --- a/mrobosub_localization_cpp/package.xml +++ b/mrobosub_localization_cpp/package.xml @@ -12,6 +12,7 @@ eigen gtsam geometry_msgs + sensor_msgs nav_msgs tf2_ros gtsam diff --git a/mrobosub_msgs/msg/Dvl.msg b/mrobosub_msgs/msg/Dvl.msg index 3a6b6b89..ea28aac0 100644 --- a/mrobosub_msgs/msg/Dvl.msg +++ b/mrobosub_msgs/msg/Dvl.msg @@ -1,2 +1,4 @@ std_msgs/Header header -float32[3] velocity \ No newline at end of file +float32[3] pd +float32[3] ad +float32 confidence From 2ce0929d4a381280a7394cb4ec6ad957559886e9 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 14 Apr 2026 20:24:26 +0000 Subject: [PATCH 24/31] Fix some logic errors in Factors and Factor helpers --- .../BluerovBarometerFactor.h | 2 +- .../mrobosub_localization_cpp/DvlOnlyFactor.h | 2 +- .../PreintegratedVelocityHelpers.h | 7 +++++- .../src/BluerovBarometerFactor.cpp | 10 ++++---- .../src/DvlOnlyFactor.cpp | 24 +++++++++---------- .../src/PreintegratedVelocityHelpers.cpp | 16 +++++++------ 6 files changed, 34 insertions(+), 27 deletions(-) diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h index d22f0acb..fd33aa1e 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -18,7 +18,7 @@ class BluerovBarometerFactor : public gtsam::NoiseModelFactor1 { ~BluerovBarometerFactor(); BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model); - gtsam::Vector evaluate_error( + gtsam::Vector evaluateError( const gtsam::Pose3 &pose, boost::optional H ) const; diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h index 83483c60..ca7d26da 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h @@ -20,7 +20,7 @@ class DvlOnlyFactor : public gtsam::NoiseModelFactorN - void serialize(ARCHIVE &ar, const unsigned int version); + void PreintegratedVelocityParameters::serialize(ARCHIVE &ar, const unsigned int version) { + ar &BOOST_SERIALIZATION_NVP(_bias_velocity_covariance); + ar &BOOST_SERIALIZATION_NVP(_bias_initial); + } public: // GTSAM uses advanced math with Eigen @@ -98,6 +101,8 @@ class PreintegratedVelocityMeasurementsDvlOnly { gtsam::Matrix get_delpij_delbias_dvl() const; gtsam::Matrix get_preintegrated_measured_covariance() const; + void update_accumulated_stats(const gtsam::imuBias::ConstantBias &bias); + gtsam::Point3 predict(const gtsam::Point3 &bias, gtsam::Matrix3 &H_bias) const; }; diff --git a/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp index e7bc78ec..2b7db057 100644 --- a/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp +++ b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp @@ -8,14 +8,16 @@ BluerovBarometerFactor::BluerovBarometerFactor(gtsam::Key key, double measured, BluerovBarometerFactor::~BluerovBarometerFactor() {} -gtsam::Vector BluerovBarometerFactor::evaluate_error( +// Expected by the parent class to be evaluateError +gtsam::Vector BluerovBarometerFactor::evaluateError( const gtsam::Pose3 &pose, boost::optional H ) const { + // If the H is requested, get the Jacobian of the Z-translation if (H) { - gtsam::Matrix36 Jt; - pose.translation(Jt); - *H = Jt.row(2); + gtsam::Matrix16 H_z; + double z = pose.z(H_z); + *H = H_z; } return (gtsam::Vector(1) << pose.z() - _measured).finished(); diff --git a/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp b/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp index cef4e1c0..9449d82c 100644 --- a/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp +++ b/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp @@ -10,7 +10,7 @@ DvlOnlyFactor::DvlOnlyFactor( const PreintegratedVelocityMeasurementsDvlOnly &pvm) : gtsam::NoiseModelFactorN( gtsam::noiseModel::Gaussian::Covariance( - _pvm._preintegrated_measured_covariance + pvm._preintegrated_measured_covariance ), pose_i, pose_j, @@ -20,7 +20,7 @@ DvlOnlyFactor::DvlOnlyFactor( DvlOnlyFactor::~DvlOnlyFactor() {} -gtsam::Vector DvlOnlyFactor::evaluate_error( +gtsam::Vector DvlOnlyFactor::evaluateError( const gtsam::Pose3 &pose_i, const gtsam::Pose3 &pose_j, const gtsam::imuBias::ConstantBias &vbias_i, @@ -38,26 +38,24 @@ gtsam::Vector DvlOnlyFactor::evaluate_error( gtsam::Point3 vbias = vbias_i.accelerometer(); gtsam::Point3 accumulated_translation = _pvm.predict(vbias, H_vbias); - // Compute the inverse of R_i and save as a Rot3 - gtsam::Vector3 residual = gtsam::Rot3(R_i - .matrix() - .transpose() - ).rotate(p_j - p_i); - residual -= accumulated_translation; + + // residual = R_i^T * (delta_p_world) - integrated_dvl_body + gtsam::Matrix3 RiT = R_i.transpose().matrix(); + gtsam::Vector3 residual = RiT * (p_j - p_i) - accumulated_translation; if (H1) { H1->resize(3, 6); - H1->block<3, 3>(0, 0) = -gtsam::Matrix3::Identity(); - H1->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); + H1->block<3, 3>(0, 0) = gtsam::Matrix3::Zero(); + H1->block<3, 3>(0, 3) = -RiT; // deriv w.r.t. p_i } if (H2) { H2->resize(3, 6); - H2->block<3, 3>(0, 0) = R_i.transpose().matrix() * R_j.matrix(); - H2->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); + H2->block<3, 3>(0, 0) = gtsam::Matrix3::Zero(); + H2->block<3, 3>(0, 3) = RiT; // deriv w.r.t. p_j } if (H3) { H3->resize(3, 6); - H3->block<3, 3>(0, 0) = H_vbias; + H3->block<3, 3>(0, 0) = -H_vbias; // acceleration bias affects prediction negatively H3->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); } return residual; diff --git a/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp b/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp index c156f0ee..e95456d1 100644 --- a/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp +++ b/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp @@ -29,14 +29,9 @@ namespace localization { _bias_initial = covariance; } - template - void PreintegratedVelocityParameters::serialize(ARCHIVE &ar, const unsigned int version) { - ar &BOOST_SERIALIZATION_NVP(_bias_velocity_covariance); - ar &BOOST_SERIALIZATION_NVP(_bias_initial); - } PreintegratedVelocityMeasurementsDvlOnly::PreintegratedVelocityMeasurementsDvlOnly() - : _preintegrated_measured_covariance(gtsam::Matrix::Zero()) {} + : _preintegrated_measured_covariance(gtsam::Matrix3::Zero()) {} PreintegratedVelocityMeasurementsDvlOnly::~PreintegratedVelocityMeasurementsDvlOnly() {} @@ -83,7 +78,7 @@ namespace localization { // TODO: Investigate this. gtsam::Matrix3 B = interpolated_rotation.matrix() * dt; - gtsam::Matrix3 dvl_covariance = fom * gtsam::I_3x3; + gtsam::Matrix3 dvl_covariance = fom * fom * gtsam::I_3x3; // Figure of Merit should be SQUARED _preintegrated_measured_covariance += B * dvl_covariance * B.transpose(); _velocity_list.push_back(linear_velocity); @@ -127,4 +122,11 @@ namespace localization { gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_preintegrated_measured_covariance() const { return _preintegrated_measured_covariance; } + + void PreintegratedVelocityMeasurementsDvlOnly::update_accumulated_stats(const gtsam::imuBias::ConstantBias &bias) { + gtsam::Matrix3 H; + _accumulated_positions = predict(bias.accelerometer(), H); + _delp_delbias_dvl = H; + } + } // namespace localization \ No newline at end of file From e5b99d665d751dfa0490d476ca429c59d2618e70 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 14 Apr 2026 20:24:51 +0000 Subject: [PATCH 25/31] Fix parameters to pass in node correctly --- .../mrobosub_localization_cpp/Parameters.h | 18 +- mrobosub_localization_cpp/src/Parameters.cpp | 209 +++++------------- 2 files changed, 70 insertions(+), 157 deletions(-) diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h index e589bbc0..b5578aa3 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h @@ -16,7 +16,7 @@ struct SensorUsage { bool is_sonar_used; bool are_cams_used; - SensorUsage(std::shared_ptr node); + SensorUsage(const std::shared_ptr &node); }; struct SensorTopics { @@ -26,7 +26,7 @@ struct SensorTopics { std::string barometer_topic; std::string sonar_topic; - SensorTopics(std::shared_ptr node); + SensorTopics(const std::shared_ptr &node); }; struct ImuParameters { @@ -43,26 +43,26 @@ struct ImuParameters { double g; double dt_imu; - ImuParameters(std::shared_ptr); + ImuParameters(const std::shared_ptr &node); }; struct ImuPreintegrationParameters { double gap_time; - ImuPreintegrationParameters(std::shared_ptr node); + ImuPreintegrationParameters(const std::shared_ptr &node); }; struct DvlParameters { double prior_bias; double fom_threshold; - DvlParameters(std::shared_ptr node); + DvlParameters(const std::shared_ptr &node); }; struct BarometerParameters { double atmospheric_pressure; - BarometerParameters(std::shared_ptr node); + BarometerParameters(const std::shared_ptr &node); }; struct OptimizationParameters { @@ -73,7 +73,7 @@ struct OptimizationParameters { double relative_error_tolerance; double absolute_error_tolerance; - OptimizationParameters(std::shared_ptr node); + OptimizationParameters(const std::shared_ptr &node); }; struct Extrinsics { @@ -83,12 +83,12 @@ struct Extrinsics { Eigen::Matrix4d T_SBa; // barometer to imu Eigen::Matrix4d T_W_WD; // world to dvl world - Extrinsics(std::shared_ptr node); + Extrinsics(const std::shared_ptr &node); }; class Parameters { public: - Parameters(std::shared_ptr node); + Parameters(const std::shared_ptr &node); ~Parameters(); // About the sensors diff --git a/mrobosub_localization_cpp/src/Parameters.cpp b/mrobosub_localization_cpp/src/Parameters.cpp index 93bf2f22..24d18b2d 100644 --- a/mrobosub_localization_cpp/src/Parameters.cpp +++ b/mrobosub_localization_cpp/src/Parameters.cpp @@ -3,158 +3,79 @@ #include namespace localization { - SensorUsage::SensorUsage(std::shared_ptr node) { - node->declare_parameter("sensor_usage.is_dvl_used", true); - is_dvl_used = node->get_parameter("sensor_usage.is_dvl_used").as_bool(); - - node->declare_parameter("sensor_usage.is_imu_used", true); - is_imu_used = node->get_parameter("sensor_usage.is_imu_used").as_bool(); - - node->declare_parameter("sensor_usage.is_barometer_used", true); - is_barometer_used = node->get_parameter("sensor_usage.is_barometer_used").as_bool(); - - node->declare_parameter("sensor_usage.is_sonar_used", false); - is_sonar_used = node->get_parameter("sensor_usage.is_sonar_used").as_bool(); - - node->declare_parameter("sensor_usage.are_cams_used", false); - are_cams_used = node->get_parameter("sensor_usage.are_cams_used").as_bool(); + SensorUsage::SensorUsage(const std::shared_ptr &node) { + is_dvl_used = node->declare_parameter("sensor_usage.is_dvl_used", false); + is_imu_used = node->declare_parameter("sensor_usage.is_imu_used", false); + is_barometer_used = node->declare_parameter("sensor_usage.is_barometer_used", false); + is_sonar_used = node->declare_parameter("sensor_usage.is_sonar_used", false); + are_cams_used = node->declare_parameter("sensor_usage.are_cams_used", false); } - SensorTopics::SensorTopics(std::shared_ptr node) { - node->declare_parameter("sensor_topics.imu_topic", "/todo"); - imu_topic = node->get_parameter("sensor_topics.imu_topic").as_string(); - - node->declare_parameter("sensor_topics.dvl_topic", "/todo"); - dvl_topic = node->get_parameter("sensor_topics.dvl_topic").as_string(); - - node->declare_parameter("sensor_topics.dvl_local_position_topic", "/todo"); - dvl_local_position_topic = node->get_parameter("sensor_topics.dvl_local_position_topic").as_string(); - - node->declare_parameter("sensor_topics.barometer_topic", "/todo"); - barometer_topic = node->get_parameter("sensor_topics.barometer_topic").as_string(); - - node->declare_parameter("sensor_topics.sonar_topic", "/unused"); - sonar_topic = node->get_parameter("sensor_topics.sonar_topic").as_string(); + SensorTopics::SensorTopics(const std::shared_ptr &node) { + imu_topic = node->declare_parameter("sensor_topics.imu_topic", "/todo"); + dvl_topic = node->declare_parameter("sensor_topics.dvl_topic", "/todo"); + dvl_local_position_topic = node->declare_parameter("sensor_topics.dvl_local_position_topic", "/todo"); + barometer_topic = node->declare_parameter("sensor_topics.barometer_topic", "/todo"); + sonar_topic = node->declare_parameter("sensor_topics.sonar_topic", "/unused"); } - ImuParameters::ImuParameters(std::shared_ptr node) { - node->declare_parameter("imu_parameters.g", 0.0); - g = node->get_parameter("imu_parameters.g").as_double(); - - node->declare_parameter("imu_parameters.acc_max", 0.0); - acc_max = node->get_parameter("imu_parameters.acc_max").as_double(); - - node->declare_parameter("imu_parameters.acc_noise_density", 0.0); - acc_noise_density = node->get_parameter("imu_parameters.acc_noise_density").as_double(); - - node->declare_parameter("imu_parameters.acc_bias_prior", 0.0); - acc_bias_prior = node->get_parameter("imu_parameters.acc_bias_prior").as_double(); - - node->declare_parameter("imu_parameters.acc_random_walk", 0.0); - acc_random_walk = node->get_parameter("imu_parameters.acc_random_walk").as_double(); - - node->declare_parameter("imu_parameters.gyro_max", 0.0); - gyro_max = node->get_parameter("imu_parameters.gyro_max").as_double(); - - node->declare_parameter("imu_parameters.gyro_noise_density", 0.0); - gyro_noise_density = node->get_parameter("imu_parameters.gyro_noise_density").as_double(); - - node->declare_parameter("imu_parameters.gyro_bias_prior", 0.0); - gyro_bias_prior = node->get_parameter("imu_parameters.gyro_bias_prior").as_double(); - - node->declare_parameter("imu_parameters.gyro_random_walk", 0.0); - gyro_random_walk = node->get_parameter("imu_parameters.gyro_random_walk").as_double(); - - node->declare_parameter("imu_parameters.integration_covarience", 0.0001); - integration_covariance = node->get_parameter("imu_parameters.integration_covarience").as_double(); - - node->declare_parameter("imu_parameters.imu_rate", 250.0); - imu_rate = node->get_parameter("imu_parameters.imu_rate").as_double(); - - dt_imu = 1 / imu_rate; + ImuParameters::ImuParameters(const std::shared_ptr &node) { + g = node->declare_parameter("imu_parameters.g", 0.0); + acc_max = node->declare_parameter("imu_parameters.acc_max", 0.0); + acc_noise_density = node->declare_parameter("imu_parameters.acc_noise_density", 0.0); + acc_bias_prior = node->declare_parameter("imu_parameters.acc_bias_prior", 0.0); + acc_random_walk = node->declare_parameter("imu_parameters.acc_random_walk", 0.0); + gyro_max = node->declare_parameter("imu_parameters.gyro_max", 0.0); + gyro_noise_density = node->declare_parameter("imu_parameters.gyro_noise_density", 0.0); + gyro_bias_prior = node->declare_parameter("imu_parameters.gyro_bias_prior", 0.0); + gyro_random_walk = node->declare_parameter("imu_parameters.gyro_random_walk", 0.0); + integration_covariance = node->declare_parameter("imu_parameters.integration_covariance", 0.0); + imu_rate = node->declare_parameter("imu_parameters.imu_rate", 0.0); + dt_imu = 1.0 / imu_rate; } - ImuPreintegrationParameters::ImuPreintegrationParameters(std::shared_ptr node) { - node->declare_parameter("imu_preintegration_parameters.gap_time", 1.0); - gap_time = node->get_parameter("imu_preintegration_parameters.gap_time").as_double(); + ImuPreintegrationParameters::ImuPreintegrationParameters(const std::shared_ptr &node) { + gap_time = node->declare_parameter("imu_preintegration_parameters.gap_time", 1.0); } - DvlParameters::DvlParameters(std::shared_ptr node) { - node->declare_parameter("dvl_parameters.prior_bias", 0.01); - prior_bias = node->get_parameter("dvl_parameters.prior_bias").as_double(); - - node->declare_parameter("dvl_parameters.fom_threshold", 0.01); - fom_threshold = node->get_parameter("dvl_parameters.fom_threshold").as_double(); + DvlParameters::DvlParameters(const std::shared_ptr &node) { + prior_bias = node->declare_parameter("dvl_parameters.prior_bias", 0.01); + fom_threshold = node->declare_parameter("dvl_parameters.fom_threshold", 0.01); } - BarometerParameters::BarometerParameters(std::shared_ptr node) { - node->declare_parameter("barometer_parameters.atmospheric_pressure", 993.3999633789062); - atmospheric_pressure = node->get_parameter("barometer_parameters.atmospheric_pressure").as_double(); + BarometerParameters::BarometerParameters(const std::shared_ptr &node) { + atmospheric_pressure =node->declare_parameter("barometer_parameters.atmospheric_pressure", 993.3999633789062); } - OptimizationParameters::OptimizationParameters(std::shared_ptr node){ - node->declare_parameter("optimization_parameters.lambda_upper_bound", 1e10); - lambda_upper_bound = node->get_parameter("optimization_parameters.lambda_upper_bound").as_double(); - - node->declare_parameter("optimization_parameters.lambda_lower_bound", 1e-10); - lambda_lower_bound = node->get_parameter("optimization_parameters.lambda_lower_bound").as_double(); - - node->declare_parameter("optimization_parameters.lambda_initial", 1e-5); - lambda_initial = node->get_parameter("optimization_parameters.lambda_initial").as_double(); - - node->declare_parameter("optimization_parameters.max_iterations", 500); - max_iterations = node->get_parameter("optimization_parameters.max_iterations").as_double(); - - node->declare_parameter("optimization_parameters.relative_error_tolerance", 1e-4); - relative_error_tolerance = node->get_parameter("optimization_parameters.relative_error_tolerance").as_double(); - - node->declare_parameter("optimization_parameters.absolute_error_tolerance", 1e-4); - absolute_error_tolerance = node->get_parameter("optimization_parameters.absolute_error_tolerance").as_double(); + OptimizationParameters::OptimizationParameters(const std::shared_ptr &node){ + lambda_upper_bound = node->declare_parameter("optimization_parameters.lambda_upper_bound", 1e10); + lambda_lower_bound = node->declare_parameter("optimization_parameters.lambda_lower_bound", 1e-10); + lambda_initial = node->declare_parameter("optimization_parameters.lambda_initial", 1e-5); + max_iterations = node->declare_parameter("optimization_parameters.max_iterations", 500); + relative_error_tolerance = node->declare_parameter("optimization_parameters.relative_error_tolerance", 1e-4); + absolute_error_tolerance = node->declare_parameter("optimization_parameters.absolute_error_tolerance", 1e-4); } - Extrinsics::Extrinsics(std::shared_ptr node) { - node->declare_parameter("imu_parameters.T_BS", std::vector()); - std::vector T_BS_flat = node->get_parameter("imu_parameters.T_BS").as_double_array(); - for(int i = 0; i < 4; ++i) { - for(int j = 0; j < 4; ++j) { - T_BS(i, j) = T_BS_flat[i * 4 + j]; - } - } - - node->declare_parameter("sonar_parameters.T_SSo", std::vector()); - std::vector T_SSo_flat = node->get_parameter("sonar_parameters.T_SSo").as_double_array(); - for(int i = 0; i < 4; ++i) { - for(int j = 0; j < 4; ++j) { - T_SSo(i, j) = T_SSo_flat[i * 4 + j]; + Extrinsics::Extrinsics(const std::shared_ptr &node) { + auto create_matrix = [](const std::vector &flat, Eigen::Matrix4d &mat) { + if (flat.size() == 16) { + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + mat(i, j) = flat[i * 4 + j]; + } + } + } else { + mat = Eigen::Matrix4d::Identity(); } - } - - node->declare_parameter("dvl_parameters.T_SD", std::vector()); - std::vector T_SD_flat = node->get_parameter("dvl_parameters.T_SD").as_double_array(); - for(int i = 0; i < 4; ++i) { - for(int j = 0; j < 4; ++j) { - T_SD(i, j) = T_SD_flat[i * 4 + j]; - } - } - - node->declare_parameter("dvl_parameters.T_W_WD", std::vector()); - std::vector T_W_WD_flat = node->get_parameter("dvl_parameters.T_W_WD").as_double_array(); - for(int i = 0; i < 4; ++i) { - for(int j = 0; j < 4; ++j) { - T_W_WD(i, j) = T_W_WD_flat[i * 4 + j]; - } - } - - node->declare_parameter("barometer_parameters.T_SBa", std::vector()); - std::vector T_SBa_flat = node->get_parameter("barometer_parameters.T_SBa").as_double_array(); - for(int i = 0; i < 4; ++i) { - for(int j = 0; j < 4; ++j) { - T_SBa(i, j) = T_SBa_flat[i * 4 + j]; - } - } + }; + create_matrix(node->declare_parameter>("imu_parameters.T_BS", {}), T_BS); + create_matrix(node->declare_parameter>("sonar_parameters.T_SSo", {}), T_SSo); + create_matrix(node->declare_parameter>("dvl_parameters.T_SD", {}), T_SD); + create_matrix(node->declare_parameter>("dvl_parameters.T_W_WD", {}), T_W_WD); + create_matrix(node->declare_parameter>("barometer_parameters.T_SBa", {}), T_SBa); } - Parameters::Parameters(std::shared_ptr node) + Parameters::Parameters(const std::shared_ptr &node) : _sensor_usage(node) , _sensor_topics(node) , _imu_params(node) @@ -163,21 +84,13 @@ namespace localization { , _barometer_params(node) , _optimization_params(node) , _extrinsics(node) { + _using_smoother = node->declare_parameter("using_smoother", true); + _using_pseudo_dvl = node->declare_parameter("using_pseudo_dvl", true); + _using_orbslam = node->declare_parameter("using_orbslam", false); - node->declare_parameter("using_orbslam", false); - _using_orbslam = node->get_parameter("using_orbslam").as_bool(); - - node->declare_parameter("num_iters", 1000); - _num_iters = node->get_parameter("num_iters").as_int(); - - node->declare_parameter("using_smoother", true); - _using_smoother = node->get_parameter("using_smoother").as_bool(); - - node->declare_parameter("keyframe_gap_time", 1.0); - _keyframe_gap_time = node->get_parameter("keyframe_gap_time").as_double(); + _keyframe_gap_time = node->declare_parameter("keyframe_gap_time", 1.0); - node->declare_parameter("using_pseudo_dvl", true); - _using_pseudo_dvl = node->get_parameter("using_pseudo_dvl").as_bool(); + _num_iters = node->declare_parameter("num_iters", 1000); _rosbag_topics.push_back(_sensor_topics.imu_topic); _rosbag_topics.push_back(_sensor_topics.dvl_topic); From a8d15a89790d5fbaa3fa9a17290f2b1449b5b8f4 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 14 Apr 2026 20:25:21 +0000 Subject: [PATCH 26/31] Fix constants based off of data sheets --- .../params/posegraph.yaml | 47 ++++++++++--------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/mrobosub_localization_cpp/params/posegraph.yaml b/mrobosub_localization_cpp/params/posegraph.yaml index 754b9142..d9f910d8 100644 --- a/mrobosub_localization_cpp/params/posegraph.yaml +++ b/mrobosub_localization_cpp/params/posegraph.yaml @@ -2,17 +2,20 @@ posegraph: ros__parameters: # IMX-5 TODO: Read the datasheet imu_parameters: - g: 0.0 - acc_max: 0.0 - acc_noise_density: 0.0 # - acc_bias_prior: 0.0 - acc_random_walk: 0.0 # [m/s^2/sqrt(Hz)] - gyro_max: 0.0 - gyro_noise_density: 0.0 # [rad/s/sqrt(Hz)] - gyro_bias_prior: 0.0 #[rad/s] - gyro_random_walk: 0.0 # [rad/s^2/sqrt(Hz)] - integration_covarience: 0.0001 - imu_rate: 250.0 #[Hz?] Verify from the datasheet + g: 9.80665 + + acc_max: 156.9 # 16g = 16 * 9.80665 + acc_noise_density: 5.88e-4 # 60 ug/sqrt(hz) = 60e-6 * 9.80665 = [m/s^2/sqrt(Hz)] + acc_bias_prior: 1e-4 # from "in-run bias stability": < 1.5 deg/hr (is an upper bound) + acc_random_walk: 2.67e-3 # 0.16/60 = [m/s^2/sqrt(Hz)] + + gyro_max: 69.81 # 4000 deg/sec * pi/180 = + gyro_noise_density: 8.73e-5 # 0.005 deg/s/sqrt(Hz) * pi/180 = [rad/s/sqrt(Hz)] + gyro_bias_prior: 1e-5 # [rad/s] + gyro_random_walk: 4.65e-5 # 0.002667 * pi/180 [rad/s^2/sqrt(Hz)] + + integration_covariance: 0.0001 + imu_rate: 1000.0 T_BS: [ 1.0000, 0.0000, 0.0000, 0.0000, @@ -32,14 +35,14 @@ posegraph: 0.0000, 0.0000, 0.0000, 1.0000, ] dvl_parameters: - T_SD: # What is this parameter? + T_SD: # Transformation from Sensor to Drone/Body (measure this!!!) [ 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, ] - T_W_WD: # What is this parameter? + T_W_WD: # Transformation from Water-Frame to World/Map (can be kept as I) [ 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, @@ -47,16 +50,16 @@ posegraph: 0.0000, 0.0000, 0.0000, 1.0000, ] prior_bias: 0.0 - fom_threshold: 0.01 + fom_threshold: 0.05 barometer_parameters: - T_SBa: # What is this parameter? + T_SBa: # Transformation from Sensor to Barometer. Z translation is important here. [ 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, ] - atmospheric_pressure: 993.3999633789062 + atmospheric_pressure: 993.3999633789062 # fine. optimization_parameters: lambda_upper_bound: 1e10 lambda_lower_bound: 1e-10 @@ -65,10 +68,10 @@ posegraph: relative_error_tolerance: 1e-4 absolute_error_tolerance: 1e-4 sensor_topics: - imu_topic: "/todo" - dvl_topic: "/todo" - dvl_local_position_topic: "/todo" - barometer_topic: "/todo" + imu_topic: "/imu" + dvl_topic: "/dvl/twist" + dvl_local_position_topic: "/dvl/local" + barometer_topic: "/depth" sonar_topic: "/unused" # left_cam_topic: # right_cam_topic: @@ -78,10 +81,10 @@ posegraph: is_barometer_used: true is_sonar_used: false are_cams_used: false - use_orb_slam: false + using_orbslam: false num_iters: 1000 using_smoother: true - kf_gap_time: 1.0 + keyframe_gap_time: 1.0 using_pseudo_dvl: true # [3/24]: As of right now, I have no plans on using the cameras. From c820847e7bf33819326ba1e32bddb9ed5435c105 Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Tue, 14 Apr 2026 20:35:57 +0000 Subject: [PATCH 27/31] Add support for new standard ros sensor types and fixes more typos --- .../mrobosub_localization_cpp/Posegraph.h | 4 +- .../mrobosub_localization_cpp/PosegraphNode.h | 34 ++++--- mrobosub_localization_cpp/src/Posegraph.cpp | 95 +++++++------------ .../src/PosegraphNode.cpp | 14 +-- 4 files changed, 62 insertions(+), 85 deletions(-) diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h index f9418efa..f7d2d4be 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -28,9 +28,11 @@ #include // std::unique_ptr namespace localization { +class PosegraphNode; // forward decl. class Posegraph { public: // Members int _index; + std::mutex &_mtx; // Smoother gtsam::ISAM2Params _smoother_parameters; @@ -94,7 +96,7 @@ class Posegraph { public: // Methods // Ctors + Dtors - Posegraph(); + Posegraph(std::mutex &mtx); ~Posegraph(); diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h index 279a5036..c0575dc0 100644 --- a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h @@ -22,6 +22,15 @@ #include #include +// Subscribers +#include +#include +#include +#include + +// TF +#include + namespace localization { class PosegraphNode : public rclcpp::Node { public: @@ -31,10 +40,10 @@ class PosegraphNode : public rclcpp::Node { std::unique_ptr _posegraph; // Define subscribers - rclcpp::Subscription::SharedPtr _imu_sub; - rclcpp::Subscription::SharedPtr _dvl_sub; - rclcpp::Subscription::SharedPtr _baro_sub; - rclcpp::Subscription::SharedPtr _dvl_local_sub; + rclcpp::Subscription::SharedPtr _imu_sub; + rclcpp::Subscription::SharedPtr _dvl_sub; + rclcpp::Subscription::SharedPtr _baro_sub; + rclcpp::Subscription::SharedPtr _dvl_local_sub; // Define publishers rclcpp::Publisher::SharedPtr _pose_pub; @@ -42,13 +51,12 @@ class PosegraphNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr _path_pub; // Define callback groups - rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _imu_callback_group; - rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _low_freq_sensor_callback_group; - rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _posegraph_callback_group; - rclcpp::CallbackGroupType::MutuallyExclusive::SharedPtr _keyframe_callback_group; + rclcpp::CallbackGroup::SharedPtr _imu_callback_group; + rclcpp::CallbackGroup::SharedPtr _low_freq_sensor_callback_group; + rclcpp::CallbackGroup::SharedPtr _posegraph_callback_group; + // Define timers rclcpp::TimerBase::SharedPtr _posegraph_timer; - rclcpp::TimerBase::SharedPtr _keyframe_timer; uint64_t _frame_count; @@ -107,14 +115,12 @@ class PosegraphNode : public rclcpp::Node { private: // methods /* TODO: What is save trajectory?? */ // bool save_trajectory(turtlmap::save_trajectory::Request &req, turtlmap::save_trajectory::Response &res); - - void imu_callback(const mrobosub_msgs::msg::Imu::SharedPtr msg); - void dvl_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg); - void baro_callback(const std_msgs::msg::Float64::SharedPtr msg); + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + void dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void baro_callback(const sensor_msgs::msg::FluidPressure::SharedPtr msg); void dvl_local_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg); void main_loop(); - void kf_loop(); void broadcast_imu_transform(); }; diff --git a/mrobosub_localization_cpp/src/Posegraph.cpp b/mrobosub_localization_cpp/src/Posegraph.cpp index 0ccd45d4..6ad6b947 100644 --- a/mrobosub_localization_cpp/src/Posegraph.cpp +++ b/mrobosub_localization_cpp/src/Posegraph.cpp @@ -9,24 +9,25 @@ namespace localization { // Ctor + Dtor -Posegraph::Posegraph() +Posegraph::Posegraph(std::mutex &mtx) : _index(0) + , _mtx(mtx) , _graph(new gtsam::NonlinearFactorGraph()) , _initial(new gtsam::Values()) , _result(new gtsam::Values()) , _preintegrated_velocity_measurements(new PreintegratedVelocityMeasurementsDvlOnly()) , _prior_imu_bias(gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0))) , _prev_dvl_odometry_time(0.0) - , _prev_dvl_odometry_rot(gtsam::Rot3()) { - // Set other parameters - set_smoother_parameters(); - set_imu_parameters(); -} + , _prev_dvl_odometry_rot(gtsam::Rot3()) + {} Posegraph::~Posegraph() {} void Posegraph::initialize_parameters(std::shared_ptr node) { _pose_graph_params = std::make_unique(node); + set_smoother_parameters(); + set_imu_parameters(); + define_transforms(); } void Posegraph::set_smoother_parameters() { @@ -37,7 +38,7 @@ void Posegraph::set_smoother_parameters() { void Posegraph::set_imu_parameters() { _preintegrated_measurement_params = boost::make_shared( - gtsam::Vector3(0, 0, _pose_graph_params->_imu_params.g) + gtsam::Vector3(0, 0, -_pose_graph_params->_imu_params.g) // Z-UP! ); _preintegrated_measurement_params->setAccelerometerCovariance( gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.acc_noise_density, 2) @@ -46,7 +47,7 @@ void Posegraph::set_imu_parameters() { gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.gyro_noise_density, 2) ); _preintegrated_measurement_params->setIntegrationCovariance( - gtsam::I_3x3 * 1e-8 + gtsam::I_3x3 * _pose_graph_params->_imu_params.integration_covariance ); _preintegrated_measurement_params->setBiasAccCovariance( gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.acc_random_walk, 2) @@ -91,6 +92,9 @@ void Posegraph::add_imu_factor() { gtsam::Symbol('b', _index), _preintegrated_measurement_params.get() ); + + // Must reset integration for next interval + _preintegrated_measurements->resetIntegrationAndSetBias(_curr_imu_bias); } void Posegraph::add_velocity_factor_with_rotation_interpolation(bool using_slerp) { @@ -211,7 +215,7 @@ void Posegraph::add_dvl_factor_imu_rotation() { int dvl_size = _curr_dvl_timestamps.size(); assert(_curr_dvl_vels.size() == _imu_rot_list.size()); - double dt_dvl; + double dt_dvl = -1; gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); @@ -233,7 +237,7 @@ void Posegraph::add_dvl_factor_imu_rotation() { if (_index > 0) { _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); - if (_index < 10000) { + if (_index < 10000) { // TODO REMOVE _graph->emplace_shared( gtsam::Symbol('x', _index - 1), gtsam::Symbol('x', _index), @@ -551,66 +555,31 @@ gtsam::Rot3 Posegraph::find_current_pose_for_dvl_vel(double time_stamp) const { } void Posegraph::calculate_interpolated_rotations(bool is_using_slerp, std::vector &interpolated_rotations) { - // Interpolate the rotations - gtsam::Rot3 prev_rotation = gtsam::Rot3(); + if (_curr_dvl_poses.empty() || _curr_dvl_timestamps.empty()) return; + + gtsam::Rot3 prev_rotation = _prev_dvl_odometry_rot; + gtsam::Rot3 curr_rotation = _curr_dvl_poses[0].rotation(); double last_local_time = _prev_dvl_odometry_time; - double curr_local_time = _curr_dvl_local_timestamps[0]; + int next_local_idx = 0; - double dt, dt_interpolated; - - // For each dvl timestamp need to interpolate the current rotation - // based off of how far between last_local_time and curr_local_time - // the curr_dvl_timestamp is - for (const double &curr_dvl_timestamp : _curr_dvl_timestamps) { - // If the current dvl timestamp is greater than the current local time - // move to the next current local time and next rotation captured - if (curr_dvl_timestamp > curr_local_time) { + + for (const double &ts : _curr_dvl_timestamps) { + while (ts > _curr_dvl_local_timestamps[next_local_idx] && + next_local_idx < _curr_dvl_local_timestamps.size() - 1) { + next_local_idx++; - prev_rotation = curr_rotation; // TODO: This was commented out??? + prev_rotation = curr_rotation; curr_rotation = _curr_dvl_poses[next_local_idx].rotation(); - last_local_time = curr_local_time; - curr_local_time = _curr_dvl_local_timestamps[next_local_idx]; - } - - // Find how far ahead of the last local timestamp this dvl timestamp is - dt = curr_dvl_timestamp - last_local_time; - dt_interpolated = dt / (curr_local_time - last_local_time); - - // The dt_interp needs to be between [0, 1] - // I suppose this could happen if curr_dvl_timestamp is _still_ - // greater than the curr_local_time. - if (dt_interpolated < 0 || dt_interpolated > 1) { - std::cout << "[calculate_interpolated_rotations] interpolated dt outside of [0, 1]\n"; - std::cout << "[calculate_interpolated_rotations] dt: " << dt << "\n"; - std::cout << "[calculate_interpolated_rotations] curr_dvl_timestamp: " << curr_dvl_timestamp << "\n"; - std::cout << "[calculate_interpolated_rotations] last_local_time: " << last_local_time << "\n"; - std::cout << "[calculate_interpolated_rotations] curr_local_time: " << curr_local_time << "\n"; - std::cout << "[calculate_interpolated_rotations] curr_rotation: " << curr_rotation << "\n"; - std::cout << "[calculate_interpolated_rotations] prev_rotation (NOT SET): " << prev_rotation << "\n"; - assert(false); - } - - gtsam::Rot3 interpolated_rotation; - if (is_using_slerp) { - interpolated_rotation = prev_rotation.slerp(dt_interpolated, curr_rotation); - std::cout << "[calculate_interpolated_rotations] interpolated_rotation: " << interpolated_rotation << "\n"; - std::cout << "[calculate_interpolated_rotations] curr_rotation: " << curr_rotation << "\n"; - std::cout << "[calculate_interpolated_rotations] prev_rotation (NOT SET): " << prev_rotation << "\n"; - std::cout << "-------------------------------------------------\n"; - } else { - assert(false); + last_local_time = _curr_dvl_local_timestamps[next_local_idx - 1]; } - interpolated_rotations.push_back(interpolated_rotation); - } - - // Check to see that we have an interpolated rotation for each dvl timestamp - if (_curr_dvl_timestamps.size() != interpolated_rotations.size()) { - std::cout << "[add_velocity_factor_with_rotation_interpolation] dvl_size and interpolated_rotations.size() mismatch\n"; - std::cout << "[add_velocity_factor_with_rotation_interpolation] dvl_size: " << _curr_dvl_timestamps.size() << "\n"; - std::cout << "[add_velocity_factor_with_rotation_interpolation] interpolated_rotations.size(): " << interpolated_rotations.size() << "\n"; - assert(false); + double denom = _curr_dvl_local_timestamps[next_local_idx] - last_local_time; + double dt_interp = (denom > 1e-6) ? (ts - last_local_time) / denom : 0.0; + + // Clamp to [0, 1] for safety + dt_interp = std::max(0.0, std::min(1.0, dt_interp)); + interpolated_rotations.push_back(prev_rotation.slerp(dt_interp, curr_rotation)); } } diff --git a/mrobosub_localization_cpp/src/PosegraphNode.cpp b/mrobosub_localization_cpp/src/PosegraphNode.cpp index 1b4642d1..29c27158 100644 --- a/mrobosub_localization_cpp/src/PosegraphNode.cpp +++ b/mrobosub_localization_cpp/src/PosegraphNode.cpp @@ -44,21 +44,21 @@ PosegraphNode::PosegraphNode() imu_options.callback_group = _imu_callback_group; auto low_freq_options = rclcpp::SubscriptionOptions(); - low_freq_options.callback_group = _low_freq_callback_group; + low_freq_options.callback_group = _low_freq_sensor_callback_group; - _imu_sub = this->create_subscription( + _imu_sub = this->create_subscription( "/imu", 10, std::bind(&PosegraphNode::imu_callback, this, std::placeholders::_1), imu_options ); - _dvl_sub = this->create_subscription( - "/dvl", 10, + _dvl_sub = this->create_subscription( + "/dvl/twist", 10, std::bind(&PosegraphNode::dvl_callback, this, std::placeholders::_1), low_freq_options ); - _baro_sub = this->create_subscription( + _baro_sub = this->create_subscription( "/depth", 10, std::bind(&PosegraphNode::baro_callback, this, std::placeholders::_1), low_freq_options @@ -190,7 +190,7 @@ void PosegraphNode::kf_loop() { _is_new_keyframe = false; } -void PosegraphNode::imu_callback(const mrobosub_msgs::msg::Imu::SharedPtr msg) { +void PosegraphNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { // This function is guaranteed to be mutually exclusive to all other // functions since it is on a separate timer. @@ -338,7 +338,7 @@ void PosegraphNode::dvl_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg) { _prev_dvl_rot = _imu_latest_rot; } -void PosegraphNode::baro_callback() { +void PosegraphNode::baro_callback(const sensor_msgs::msg::FluidPressure::SharedPtr msg) { std::unique_lock lock(_mtx); if (!_is_rot_initialized) { _cond_var.wait(lock, [_imu_init_rot] { return _imu_init_rot.size() < 5; }); From f8ca4e4f163fdfd716f3c484b54029e711b8d3ce Mon Sep 17 00:00:00 2001 From: Muskaan Mittal Date: Tue, 14 Apr 2026 19:41:57 -0400 Subject: [PATCH 28/31] esp32 newlines & reads from esp32 now --- mrobosub_hal/mrobosub_hal/esp32_thruster.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/mrobosub_hal/mrobosub_hal/esp32_thruster.py b/mrobosub_hal/mrobosub_hal/esp32_thruster.py index ccddab70..8c174053 100644 --- a/mrobosub_hal/mrobosub_hal/esp32_thruster.py +++ b/mrobosub_hal/mrobosub_hal/esp32_thruster.py @@ -38,20 +38,22 @@ def __init__(self): def connect(self) -> bool: try: - self.serial = Serial(self.port, timeout=0.5, write_timeout=0.5) - enable_all() + self.serial = Serial(self.port, timeout=0, write_timeout=0.5) + self.enable_all() except SerialException as e: self.get_logger().info(f"Could not connect to esp32_thruster: {e}") return False return True def write(self, data: str) -> bool: + data = data + "\n" if self.serial is None: success = self.connect() if not success or self.serial is None: + self.get_logger().error("Serial is not connected. AHHHH!") return False try: - self.serial.write(data) + self.serial.write(data.encode()) return True except SerialException as e: self.get_logger().info(f"write error: {e}") @@ -90,7 +92,7 @@ def send_power(self, pin: int, percent_raw: float) -> int: def send_enable_disable(self, pin: int, enable: bool) -> int: - if(enable == true): + if(enable): msg = "ENABLE:TRUE" else: msg = "ENABLE:FALSE" @@ -106,7 +108,7 @@ def send_enable_disable(self, pin: int, enable: bool) -> int: def enable_all(self): for i in range(NUM_PINS): - send_enable_disable(i, true) + self.send_enable_disable(i, True) def send_estop(self): @@ -118,7 +120,7 @@ def handle_emergency_stop( self, req: SetBool.Request, res: SetBool.Response ) -> SetBool.Response: if req.data: - send_estop + self.send_estop() res.success = True return res @@ -131,6 +133,11 @@ def loop(self): for i in range(NUM_PINS): self.send_power(i, self.thruster_outputs[i]) + if self.serial is not None: + data = self.serial.read_all() + if data is not None: + self.get_logger().info(f"data read from esp32: {str(data)}") + def main(): rclpy.init() From aae37ce34991e36a794da624922d02da758381df Mon Sep 17 00:00:00 2001 From: Zayn Baig Date: Wed, 15 Apr 2026 01:11:05 +0000 Subject: [PATCH 29/31] Fixed imu --- mrobosub_hal_imu/CMakeLists.txt | 40 ++++++++++++------------------- mrobosub_hal_imu/package.xml | 1 + mrobosub_hal_imu/src/imu_node.cpp | 25 +++++++++---------- 3 files changed, 29 insertions(+), 37 deletions(-) diff --git a/mrobosub_hal_imu/CMakeLists.txt b/mrobosub_hal_imu/CMakeLists.txt index 725cd9ce..c5b8b55f 100644 --- a/mrobosub_hal_imu/CMakeLists.txt +++ b/mrobosub_hal_imu/CMakeLists.txt @@ -3,48 +3,38 @@ project(mrobosub_hal_imu) find_package(PkgConfig REQUIRED) pkg_check_modules(LIBUSB REQUIRED libusb-1.0) +find_package(Threads REQUIRED) -add_subdirectory(inertial-sense-sdk) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(mrobosub_msgs REQUIRED) include_directories( - inertial-sense-sdk/src - inertial-sense-sdk/src/yaml-cpp + ${CMAKE_CURRENT_SOURCE_DIR}/inertial-sense-sdk/src + ${CMAKE_CURRENT_SOURCE_DIR}/inertial-sense-sdk/src/yaml-cpp ${LIBUSB_INCLUDE_DIRS} ) -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -find_package(rclcpp REQUIRED) -find_package(mrobosub_msgs REQUIRED) - +add_subdirectory(inertial-sense-sdk) add_executable(imu_node src/imu_node.cpp) -ament_target_dependencies(imu_node rclcpp mrobosub_msgs) -target_link_libraries(imu_node InertialSenseSDK ${LIBUSB_LIBRARIES}) -install(TARGETS - imu_node - DESTINATION lib/${PROJECT_NAME} -) +ament_target_dependencies(imu_node rclcpp mrobosub_msgs) -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ +target_link_libraries(imu_node + InertialSenseSDK + ${LIBUSB_LIBRARIES} + Threads::Threads ) +install(TARGETS imu_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/mrobosub_hal_imu/package.xml b/mrobosub_hal_imu/package.xml index 8266cde2..7603f5dc 100644 --- a/mrobosub_hal_imu/package.xml +++ b/mrobosub_hal_imu/package.xml @@ -14,6 +14,7 @@ rclcpp mrobosub_msgs + sensor_msgs ament_cmake diff --git a/mrobosub_hal_imu/src/imu_node.cpp b/mrobosub_hal_imu/src/imu_node.cpp index e8533a4d..514f0e29 100644 --- a/mrobosub_hal_imu/src/imu_node.cpp +++ b/mrobosub_hal_imu/src/imu_node.cpp @@ -6,8 +6,7 @@ #include "../inertial-sense-sdk/src/data_sets.h" #include -// #include -#include +#include #include #include @@ -47,6 +46,8 @@ struct PimuStorage { PimuStorage pimu_storage; +#define TIME_EPSILON 0.01 + /** * Publishes: * /imu Imu @@ -78,12 +79,12 @@ int main(int argc, char **argv) switch(data->hdr.id) { case DID_GPS1_POS: { auto gps = reinterpret_cast(data->ptr); - pimu.tow_offset = gps->towOffset; + pimu_storage.tow_offset = gps->towOffset; break; } case DID_PIMU: { - pimu_storage.pimu = reinterpret_cast(data->ptr); + pimu_storage.pimu = *reinterpret_cast(data->ptr); pimu_storage.has_pimu = true; break; } @@ -96,16 +97,16 @@ int main(int argc, char **argv) if (std::abs(pimu_time_to_tow - ins->timeOfWeek) < TIME_EPSILON) { auto time_stamp = timeManager.ros_time_from_start_time(ins->timeOfWeek); - const auto div = 1.0f/data->dt; + const auto div = 1.0f/pimu_storage.pimu.dt; sensor_msgs::msg::Imu msg; msg.header.stamp = time_stamp; msg.header.frame_id = "imu_link"; - msg.orientation.w = data->qn2b[0]; - msg.orientation.x = data->qn2b[1]; - msg.orientation.y = data->qn2b[2]; - msg.orientation.z = data->qn2b[3]; + msg.orientation.w = ins->qn2b[0]; + msg.orientation.x = ins->qn2b[1]; + msg.orientation.y = ins->qn2b[2]; + msg.orientation.z = ins->qn2b[3]; msg.angular_velocity.x = pimu_storage.pimu.theta[0] * div; msg.angular_velocity.y = pimu_storage.pimu.theta[1] * div; @@ -122,9 +123,9 @@ int main(int argc, char **argv) msg.linear_acceleration_covariance[4] = acc_var; msg.linear_acceleration_covariance[8] = acc_var; - msg.angular_acceleration_covariance[0] = gyro_var; - msg.angular_acceleration_covariance[4] = gyro_var; - msg.angular_acceleration_covariance[8] = gyro_var; + msg.angular_velocity_covariance[0] = gyro_var; + msg.angular_velocity_covariance[4] = gyro_var; + msg.angular_velocity_covariance[8] = gyro_var; imu_pub->publish(msg); } From d5c79354d2b4f0a2b94f7dbadb67ea57c7e79b20 Mon Sep 17 00:00:00 2001 From: Jetson Date: Tue, 14 Apr 2026 21:15:01 -0400 Subject: [PATCH 30/31] we in the wter --- .vimrc | 2 +- mrobosub_hal/mrobosub_hal/arduino.py | 2 +- mrobosub_hal/mrobosub_hal/esp32_thruster.py | 10 +++++++--- mrobosub_hal/mrobosub_hal/thruster_controller.py | 5 ++++- mrobosub_hal/params/thruster_controller_params.yaml | 2 +- mrobosub_hal/setup.py | 2 +- mrobosub_hal_imu/launch/imu_launch.xml | 2 +- mrobosub_perception/launch/ml_executor_launch.xml | 7 ++++--- 8 files changed, 20 insertions(+), 12 deletions(-) diff --git a/.vimrc b/.vimrc index cd6e24eb..17a8cc1b 100644 --- a/.vimrc +++ b/.vimrc @@ -2,5 +2,5 @@ syntax enable set tabstop=4 set shiftwidth=4 set expandtab -set visualbell +set novisualbell set number diff --git a/mrobosub_hal/mrobosub_hal/arduino.py b/mrobosub_hal/mrobosub_hal/arduino.py index ad528cf2..e41430a4 100755 --- a/mrobosub_hal/mrobosub_hal/arduino.py +++ b/mrobosub_hal/mrobosub_hal/arduino.py @@ -8,7 +8,7 @@ FREQUENCY = 60 # times per second BAUD_RATE = 9600 -CONNECTION_NAME = "/dev/ttyACM0" +CONNECTION_NAME = "/dev/ttyACM1" class Arduino(Node): def __init__(self): diff --git a/mrobosub_hal/mrobosub_hal/esp32_thruster.py b/mrobosub_hal/mrobosub_hal/esp32_thruster.py index 8c174053..d76aae01 100644 --- a/mrobosub_hal/mrobosub_hal/esp32_thruster.py +++ b/mrobosub_hal/mrobosub_hal/esp32_thruster.py @@ -10,7 +10,7 @@ from typing import Optional -NUM_PINS = 12 +NUM_PINS = 8 FREQUENCY = 50 @@ -22,7 +22,7 @@ class ESP32_Thruster(Node): def __init__(self): super().__init__("esp32_thruster") self.get_logger().info("Launched esp32_thruster node") - self.port = "/dev/thruster_esp" #IMPORTANT: DETERMINE ACTUAL PORT + self.port = "/dev/ttyACM2" #IMPORTANT: DETERMINE ACTUAL PORT self.thruster_outputs = [0] * NUM_PINS self.serial: Serial | None = None self.connect() @@ -92,6 +92,7 @@ def send_power(self, pin: int, percent_raw: float) -> int: def send_enable_disable(self, pin: int, enable: bool) -> int: + self.get_logger().info(f"Send enable {enable} to pin {pin}") if(enable): msg = "ENABLE:TRUE" else: @@ -128,6 +129,8 @@ def eps32_thruster_commands_callback(self, msg: ThrusterCommands): for i in range(NUM_PINS): if msg.valid[i]: self.thruster_outputs[i] = msg.pins[i] + if msg.pins[i] != 0: + self.get_logger().info(f"The motor at index {i} is running at value {msg.pins[i]}") def loop(self): for i in range(NUM_PINS): @@ -136,7 +139,8 @@ def loop(self): if self.serial is not None: data = self.serial.read_all() if data is not None: - self.get_logger().info(f"data read from esp32: {str(data)}") + pass + # self.get_logger().info(f"data read from esp32: {str(data)}") def main(): diff --git a/mrobosub_hal/mrobosub_hal/thruster_controller.py b/mrobosub_hal/mrobosub_hal/thruster_controller.py index a97804c9..3812b5a1 100755 --- a/mrobosub_hal/mrobosub_hal/thruster_controller.py +++ b/mrobosub_hal/mrobosub_hal/thruster_controller.py @@ -40,9 +40,12 @@ def __init__(self): params = [Param('thruster_reverse', rclpy.Parameter.Type.BOOL_ARRAY, "List of booleans indicating whether each thruster is reversed"), Param('thruster_motor_id', rclpy.Parameter.Type.INTEGER_ARRAY, "List of motor IDs for each thruster on the thruster controller") ] - + self.declare_params(params) + for i in range(NUM_MOTORS): + self.get_logger().info(f"Motor index {i} is {self.thruster_motor_id[i]}") + def handle_emergency_stop( self, req: SetBool.Request, res: SetBool.Response diff --git a/mrobosub_hal/params/thruster_controller_params.yaml b/mrobosub_hal/params/thruster_controller_params.yaml index ec41bf60..6dc6f92d 100644 --- a/mrobosub_hal/params/thruster_controller_params.yaml +++ b/mrobosub_hal/params/thruster_controller_params.yaml @@ -1,4 +1,4 @@ thruster_controller: ros__parameters: thruster_reverse: [true, true, true, true, false, false, true, true] - thruster_motor_id: [2, 10, 8, 1, 4, 0, 3, 5] + thruster_motor_id: [3, 4, 1, 7, 2, 6, 0, 5] diff --git a/mrobosub_hal/setup.py b/mrobosub_hal/setup.py index 78b072d8..ebbdc03d 100644 --- a/mrobosub_hal/setup.py +++ b/mrobosub_hal/setup.py @@ -28,7 +28,7 @@ "dvl_publisher = mrobosub_hal.dvl_publisher:main", "botcam = mrobosub_hal.botcam:main", "zed = mrobosub_hal.zed:main", - "esp32_thruster = mrobosub_hal.esp32_controller:main", + "esp32_thruster = mrobosub_hal.esp32_thruster:main", "arduino = mrobosub_hal.arduino:main" ], }, diff --git a/mrobosub_hal_imu/launch/imu_launch.xml b/mrobosub_hal_imu/launch/imu_launch.xml index a206092e..5604533f 100644 --- a/mrobosub_hal_imu/launch/imu_launch.xml +++ b/mrobosub_hal_imu/launch/imu_launch.xml @@ -1,5 +1,5 @@ - + diff --git a/mrobosub_perception/launch/ml_executor_launch.xml b/mrobosub_perception/launch/ml_executor_launch.xml index f0ff4038..7dcdd1c8 100644 --- a/mrobosub_perception/launch/ml_executor_launch.xml +++ b/mrobosub_perception/launch/ml_executor_launch.xml @@ -1,4 +1,5 @@ - - - + + + + \ No newline at end of file From 7bcd629be3c291a07c1c7da3801208a55905119a Mon Sep 17 00:00:00 2001 From: Jetson Date: Tue, 21 Apr 2026 20:08:03 -0400 Subject: [PATCH 31/31] running ml works finallyyy, can detect sawfish etc --- mrobosub_perception/README.md | 43 ++++ mrobosub_perception/_CMakeLists.txt | 208 ------------------ .../mrobosub_perception/ml_executor.py | 21 +- .../mrobosub_perception/ml_srv.py | 12 +- mrobosub_perception/setup.py | 13 ++ 5 files changed, 77 insertions(+), 220 deletions(-) create mode 100644 mrobosub_perception/README.md delete mode 100644 mrobosub_perception/_CMakeLists.txt diff --git a/mrobosub_perception/README.md b/mrobosub_perception/README.md new file mode 100644 index 00000000..1ede8e50 --- /dev/null +++ b/mrobosub_perception/README.md @@ -0,0 +1,43 @@ +# How to install torch, torchvision on fresh jetson + +Updated Apr 2026 by Muskaan Mittal (muskaan@umich.edu) + +Sorry I don't have more detailed instructions, installing this drove me insane. + +Basically, you have to install cuda (if cuda wasn't already installed when the jetpack was), torch, and torchvision + +## STEP 1. Find out Jetpack version +At time of writing doc, we are using jetpack 6.1; which works well with CUDA 12.6 + +## STEP 2: Install appropriate CUDA + +## STEP 3: Install appropriate torch. You CANNOT do pip install torch +I used the instructions here: https://docs.nvidia.com/deeplearning/frameworks/install-pytorch-jetson-platform/index.html +and found out the appropriate version of torch using some hunting + +the export TORCH_INSTALL url came out to https://developer.download.nvidia.com/compute/redist/jp/v61/pytorch/ (... insert appropriate version) + +## STEP 4: Install other dependencies and test torch installation +Test installation by running this: +python3 -c "import torch; print(torch.__version__)" +python3 -c "import torch; print(torch.cuda.is_available())" + +You'll probably run into a whole host of python packages you don't have. Just sudo apt/ pip install them. + +## STEP 5: Install torchvision +There are pre-built wheels but there were none that matched CUDA 12.6, so I built from source which took about 30 min + +These are the instructions Claude gave me; some iteration of them worked successfully: + +### Install dependencies +sudo apt-get install -y libjpeg-dev libpng-dev libtiff-dev libavcodec-dev libavformat-dev libswscale-dev + +### Clone torchvision +git clone https://github.com/pytorch/vision.git +cd vision +git checkout tags/v0.20.0 + +### Build and install +sudo python3 setup.py install + +You can test whether this worked by opening up a python shell and making sure "import torchvision" runs successfully. \ No newline at end of file diff --git a/mrobosub_perception/_CMakeLists.txt b/mrobosub_perception/_CMakeLists.txt deleted file mode 100644 index 0e3a0428..00000000 --- a/mrobosub_perception/_CMakeLists.txt +++ /dev/null @@ -1,208 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mrobosub_perception) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS mrobosub_msgs dynamic_reconfigure) -generate_dynamic_reconfigure_options( - cfg/hsv_params.cfg - cfg/pathmarker_params.cfg - cfg/rectify_params.cfg - cfg/dummy_botcam_params.cfg -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html - -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( - # FILES - # ObjectPosition.srv - # ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS mrobosub_msgs dynamic_reconfigure - DEPENDS python-numpy python-cv2 python-cv_bridge -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/mrobosub_perception.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/mrobosub_perception_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobosub_perception.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/mrobosub_perception/mrobosub_perception/ml_executor.py b/mrobosub_perception/mrobosub_perception/ml_executor.py index 3b518fe6..b06b43b8 100755 --- a/mrobosub_perception/mrobosub_perception/ml_executor.py +++ b/mrobosub_perception/mrobosub_perception/ml_executor.py @@ -15,10 +15,10 @@ def load_yolo(): # load model - path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../..")) - yolo_path = os.path.join(path, "yolov5") + path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) + yolo_path = os.path.join(path, "src/mrobosub_perception/yolov5") - model_path = os.path.join(path, "models/mar2026_best.pt") + model_path = os.path.expanduser('~/ros2_ws/src/mrobosub_perception/models/mar2026_best.pt') print(yolo_path) print(model_path) model = torch.hub.load( @@ -27,6 +27,15 @@ def load_yolo(): model.conf = 0.1 # NMS confidence threshold return model +def make_detection(d) -> Detection: + det = Detection() + det.left = float(d[0]) + det.top = float(d[1]) + det.right = float(d[2]) + det.bottom = float(d[3]) + det.confidence = float(d[4]) + det.classification = int(d[5]) + return det class MlExecutor(Node): def __init__(self): @@ -57,9 +66,9 @@ def zed_callback(self, image: Image): print(f"TIME: {time.time() - start}") message = Detections( - detections=(Detection(*d) for d in detections), - width=width, - height=height, + detections=[make_detection(d) for d in detections], + width=float(width), + height=float(height), ) self.detection_pub.publish(message) diff --git a/mrobosub_perception/mrobosub_perception/ml_srv.py b/mrobosub_perception/mrobosub_perception/ml_srv.py index d5060594..5ddbe2c2 100755 --- a/mrobosub_perception/mrobosub_perception/ml_srv.py +++ b/mrobosub_perception/mrobosub_perception/ml_srv.py @@ -40,7 +40,7 @@ def __init__(self): name = target.name.lower() setattr(self, f"{name}_srv", self.create_service(ObjectPosition, f"object_position/{name}", - lambda _, response: self.handle_obj_request(idx, response), # the _ is the request which is unused + lambda _, response, i=idx: self.handle_obj_request(i, response), )) # eg: will define self.gate_red_srv = a service "object_position/gate_red" whose handler is handle_obj_request(0, resp) # where 0 is the index of GATE_RED in Targets @@ -96,11 +96,11 @@ def detections_callback(self, detections: Detections): d_y = y_pos - (height / 2) object_pos.found = True - object_pos.x_position = x_pos - object_pos.y_position = y_pos - object_pos.x_theta = (d_x * fov_x) / width - object_pos.y_theta = (d_y * fov_y) / height - object_pos.confidence = d.confidence + object_pos.x_position = float(x_pos) + object_pos.y_position = float(y_pos) + object_pos.x_theta = float((d_x * fov_x)) / width + object_pos.y_theta = float((d_y * fov_y)) / height + object_pos.confidence = float(d.confidence) idx: int = d.classification diff --git a/mrobosub_perception/setup.py b/mrobosub_perception/setup.py index 6d681421..73853481 100644 --- a/mrobosub_perception/setup.py +++ b/mrobosub_perception/setup.py @@ -1,8 +1,20 @@ from setuptools import find_packages, setup import glob +import os package_name = 'mrobosub_perception' +def get_data_files(src_dir, dest_prefix): + result = [] + for dirpath, dirnames, filenames in os.walk(src_dir): + dirnames[:] = [d for d in dirnames if d != '__pycache__'] + if not filenames: + continue + files = [os.path.join(dirpath, f) for f in filenames if not f.endswith('.pyc')] + dest = os.path.join(dest_prefix, dirpath) + result.append((dest, files)) + return result + setup( name=package_name, version='0.0.0', @@ -13,6 +25,7 @@ ('share/' + package_name, ['package.xml']), (f"share/{package_name}/launch", glob.glob("launch/*")), (f"share/{package_name}/params", glob.glob("params/*")), + *get_data_files('yolov5', 'share/' + package_name), ], install_requires=['setuptools'], zip_safe=True,