-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
add support for kinematics measurement in realsense device itself.
- Loading branch information
wei.wei
committed
Nov 29, 2023
1 parent
52d80ca
commit 4fb634e
Showing
10 changed files
with
413 additions
and
55 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,109 @@ | ||
| /* | ||
| * Copyright (c) 2022-2023, William Wei. All rights reserved. | ||
| * | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
| #include "kinematics.h" | ||
| #include <iostream> | ||
|
|
||
| namespace aruconavi { | ||
| using float3 = vec3<float>; | ||
|
|
||
| bool check_imu_is_supported() { | ||
| bool found_gyro = false; | ||
| bool found_accel = false; | ||
| rs2::context ctx; | ||
| for (auto dev : ctx.query_devices()) { | ||
| // The same device should support gyro and accel | ||
| found_gyro = false; | ||
| found_accel = false; | ||
| for (auto sensor : dev.query_sensors()) { | ||
| for (auto profile : sensor.get_stream_profiles()) { | ||
| if (profile.stream_type() == RS2_STREAM_GYRO) | ||
| found_gyro = true; | ||
|
|
||
| if (profile.stream_type() == RS2_STREAM_ACCEL) | ||
| found_accel = true; | ||
| } | ||
| } | ||
| if (found_gyro && found_accel) | ||
| break; | ||
| } | ||
| return found_gyro && found_accel; | ||
| } | ||
|
|
||
| void RotationEstimator::process_gyro(rs2_vector gyro_data, double ts) { | ||
| if (first_gyro) { | ||
| first_gyro = false; | ||
| last_ts_gyro = ts; | ||
| } | ||
| // Holds the change in angle, as calculated from gyro | ||
| float3 gyro_angle; | ||
|
|
||
| gyro_angle.e[0] = gyro_data.x; // pitch | ||
| gyro_angle.e[1] = gyro_data.y; // yaw | ||
| gyro_angle.e[2] = gyro_data.z; // roll | ||
|
|
||
| // Compute the difference between arrival times of previous and current gyro | ||
| // frames | ||
| double dt_gyro = (ts - last_ts_gyro) / 1000.0; | ||
| last_ts_gyro = ts; | ||
|
|
||
| // Change in angle equals gyro measures * time passed since last measurement | ||
| gyro_angle = gyro_angle * static_cast<float>(dt_gyro); | ||
| // Apply the calculated change of angle to the current angle (theta) | ||
| std::lock_guard<std::mutex> lock(theta_mutex); | ||
| theta.add(-gyro_angle.z(), -gyro_angle.y(), -gyro_angle.x()); | ||
| } | ||
|
|
||
| void RotationEstimator::process_accel(rs2_vector accel_data) { | ||
| float3 accel_angle; | ||
| // Calculate rotation angle from accelerometer data | ||
| accel_angle.e[2] = atan2(accel_data.y, accel_data.z); | ||
| accel_angle.e[0] = atan2(accel_data.x, sqrt(accel_data.y * accel_data.y + | ||
| accel_data.z * accel_data.z)); | ||
| // If it is the first iteration, set initial pose of camera according to | ||
| // accelerometer data (note the different handling for Y axis) | ||
| std::lock_guard<std::mutex> lock(theta_mutex); | ||
| if (first_accel) { | ||
| first_accel = false; | ||
| theta = accel_angle; | ||
| // Since we can't infer the angle around Y axis using accelerometer data, | ||
| // we'll use PI as a convetion for the initial pose | ||
| theta.e[1] = M_PI; | ||
| } else { | ||
| /* | ||
| Apply Complementary Filter: | ||
| - high-pass filter = theta * alpha: allows short-duration signals to | ||
| pass through while filtering out signals that are steady over time, is | ||
| used to cancel out drift. | ||
| - low-pass filter = accel * (1- alpha): lets through long term | ||
| changes, filtering out short term fluctuations | ||
| */ | ||
| theta.e[0] = theta.e[0] * alpha + accel_angle.e[0] * (1 - alpha); | ||
| theta.e[2] = theta.e[2] * alpha + accel_angle.e[2] * (1 - alpha); | ||
| } | ||
| } | ||
|
|
||
| float3 RotationEstimator::get_theta() { | ||
| std::lock_guard<std::mutex> lock(theta_mutex); | ||
| return theta; | ||
| } | ||
|
|
||
| void RotationEstimator::print_theta() { | ||
| float3 cur_rot = get_theta(); | ||
| std::cout << "realsense device rotate in yall: " << cur_rot.y() << "\n" | ||
| << "realsense device rotate in pitch: " << cur_rot.x() << "\n" | ||
| << "realsense device rotate in roll: " << cur_rot.z() << std::endl; | ||
| } | ||
| } // namespace aruconavi |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| /* | ||
| * Copyright (c) 2022-2023, William Wei. All rights reserved. | ||
| * | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
|
|
||
| #ifndef ARUCO_NAVI_KINEMATICS_H | ||
| #define ARUCO_NAVI_KINEMATICS_H | ||
| #include <librealsense2/rs.hpp> | ||
| #include <mutex> | ||
| #include "mathtypes.h" | ||
|
|
||
| namespace aruconavi { | ||
|
|
||
| class RotationEstimator { | ||
| using float3 = vec3<float>; | ||
| // theta is the angle of camera rotation in x, y and z components | ||
| float3 theta; | ||
| std::mutex theta_mutex; | ||
| /* alpha indicates the part that gyro and accelerometer take in computation | ||
| of theta; higher alpha gives more weight to gyro, but too high values cause | ||
| drift; lower alpha gives more weight to accelerometer, which is more | ||
| sensitive to disturbances */ | ||
| float alpha = 0.98f; | ||
| bool first_gyro = true; | ||
| bool first_accel = true; | ||
| // keeps the arrival time of previous gyro frame | ||
| double last_ts_gyro = 0; | ||
|
|
||
| public: | ||
| void process_gyro(rs2_vector gyro_data, double ts); | ||
| void process_accel(rs2_vector accel_data); | ||
| float3 get_theta(); | ||
| void print_theta(); | ||
| }; | ||
|
|
||
| bool check_imu_is_supported(); | ||
| } // namespace aruconavi | ||
| #endif // ARUCO_NAVI_KINEMATICS_H |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,20 @@ | ||
| /* | ||
| * Copyright (c) 2022-2023, William Wei. All rights reserved. | ||
| * | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
|
|
||
| #ifndef ARUCO_NAVI_MAGIC_H | ||
| #define ARUCO_NAVI_MAGIC_H | ||
| static auto anyone = [](auto&& k, auto&&... args) ->bool { return ((args == k) || ...); }; | ||
| #endif // ARUCO_NAVI_MAGIC_H |
Oops, something went wrong.