Skip to content

Fix Fusion.h build error in shared header #396

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
JanuszBedkowski merged 3 commits into from
Mar 9, 2026
Merged

Fix Fusion.h build error in shared header #396

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
012278b
Fix Fusion.h build error in targets that include lidar_odometry_utils.h
Jakubach Mar 9, 2026
60d141f
Apply clang-format-21 to pass CI check
Jakubach Mar 9, 2026
b798b4e
Fix MSVC C4576 compound literal and add Fusion to pybind target
Jakubach Mar 9, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
292 changes: 152 additions & 140 deletions .../lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
View file
Open in desktop

Large diffs are not rendered by default.

17 changes: 12 additions & 5 deletions apps/lidar_odometry_step_1/lidar_odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <mutex>

#include <Core/system_info.hpp>
#include <Fusion.h>

namespace fs = std::filesystem;

Expand Down Expand Up @@ -344,7 +345,9 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa
{
const double g = 9.81;
vqf_real_t gyr_vqf[3] = { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) };
vqf_real_t acc_vqf[3] = { static_cast<double>(acc.x()) * g, static_cast<double>(acc.y()) * g, static_cast<double>(acc.z()) * g };
vqf_real_t acc_vqf[3] = { static_cast<double>(acc.x()) * g,
static_cast<double>(acc.y()) * g,
static_cast<double>(acc.z()) * g };

vqf.update(gyr_vqf, acc_vqf);

Expand Down Expand Up @@ -393,7 +396,9 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa
if (params.use_removie_imu_bias_from_first_stationary_scan && imu_data.size() > 1000)
{
std::vector<double> gyr_x, gyr_y, gyr_z;
gyr_x.reserve(1000); gyr_y.reserve(1000); gyr_z.reserve(1000);
gyr_x.reserve(1000);
gyr_y.reserve(1000);
gyr_z.reserve(1000);
for (int i = 0; i < 1000; i++)
{
const auto& [timestamp_pair, gyr, acc] = imu_data[i];
Expand All @@ -404,7 +409,9 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa
std::nth_element(gyr_x.begin(), gyr_x.begin() + 500, gyr_x.end());
std::nth_element(gyr_y.begin(), gyr_y.begin() + 500, gyr_y.end());
std::nth_element(gyr_z.begin(), gyr_z.begin() + 500, gyr_z.end());
bias_gyr_x = gyr_x[500]; bias_gyr_y = gyr_y[500]; bias_gyr_z = gyr_z[500];
bias_gyr_x = gyr_x[500];
bias_gyr_y = gyr_y[500];
bias_gyr_z = gyr_z[500];
params.estimated_gyro_bias_dps = Eigen::Vector3d(bias_gyr_x, bias_gyr_y, bias_gyr_z);

std::cout << "------------------\nGYRO BIAS\n"
Expand Down Expand Up @@ -453,8 +460,8 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa
if (counter % 100 == 0)
{
const FusionEuler euler = FusionQuaternionToEuler(quat);
std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw
<< " [" << counter << " of " << imu_data.size() << "]" << std::endl;
std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw << " ["
<< counter << " of " << imu_data.size() << "]" << std::endl;
}
}
}
Expand Down
284 changes: 150 additions & 134 deletions apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
View file
Open in desktop

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions apps/lidar_odometry_step_1/lidar_odometry_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
#include <common/include/cauchy.h>
#include <laszip/laszip_api.h>
#include <nlohmann/json.hpp>
#include <Fusion.h>
#include <vqf.hpp>

#include <Core/ndt.h>
Expand Down Expand Up @@ -195,7 +194,8 @@ struct LidarOdometryParams

// IMU preintegration
bool use_imu_preintegration = false;
int imu_preintegration_method = 6; // 0=euler_body, 1=trapezoidal_body, 2=euler_gravity, 3=trapezoidal_gravity, 4=kalman, 5=euler_ahrs, 6=trapezoidal_ahrs, 7=kalman_ahrs
int imu_preintegration_method = 6; // 0=euler_body, 1=trapezoidal_body, 2=euler_gravity, 3=trapezoidal_gravity, 4=kalman, 5=euler_ahrs,
// 6=trapezoidal_ahrs, 7=kalman_ahrs

// ablation study
bool ablation_study_use_planarity = false;
Expand Down
15 changes: 7 additions & 8 deletions apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2434,16 +2434,16 @@ bool process_worker_step_1(

if (total_imu_time > 0.0)
{
bool uses_ahrs_velocity = (method == PreintegrationMethod::euler_gravity_ahrs_vel ||
method == PreintegrationMethod::trapezoidal_gravity_ahrs_vel ||
method == PreintegrationMethod::kalman_gravity_ahrs_vel);
bool uses_ahrs_velocity =
(method == PreintegrationMethod::euler_gravity_ahrs_vel ||
method == PreintegrationMethod::trapezoidal_gravity_ahrs_vel ||
method == PreintegrationMethod::kalman_gravity_ahrs_vel);

if (uses_ahrs_velocity)
{
// Methods 5-7: SM-independent velocity
// Direction from VQF AHRS, speed from motion model displacement
Eigen::Vector3d prev_mm_displacement =
prev_worker_data.intermediate_trajectory_motion_model.back().translation() -
Eigen::Vector3d prev_mm_displacement = prev_worker_data.intermediate_trajectory_motion_model.back().translation() -
prev_worker_data.intermediate_trajectory_motion_model.front().translation();
double speed = prev_mm_displacement.norm() / total_imu_time;

Expand All @@ -2457,8 +2457,7 @@ bool process_worker_step_1(
else
{
// Methods 0-4: velocity from previous SM trajectory
Eigen::Vector3d prev_displacement =
prev_worker_data.intermediate_trajectory.back().translation() -
Eigen::Vector3d prev_displacement = prev_worker_data.intermediate_trajectory.back().translation() -
prev_prev_worker_data.intermediate_trajectory.back().translation();
imu_params.initial_velocity = prev_displacement / total_imu_time;
}
Expand All @@ -2472,7 +2471,7 @@ bool process_worker_step_1(
// No preintegration: simple velocity from previous two workers
mean_shift = (prev_worker_data.intermediate_trajectory.back().translation() -
prev_prev_worker_data.intermediate_trajectory.back().translation()) /
static_cast<double>(prev_worker_data.intermediate_trajectory.size());
static_cast<double>(prev_worker_data.intermediate_trajectory.size());
}

if (mean_shift.norm() > 1.0)
Expand Down
5 changes: 3 additions & 2 deletions apps/mandeye_raw_data_viewer/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,14 @@ target_include_directories(
${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
${LASZIP_INCLUDE_DIR}/LASzip/include
${THIRDPARTY_DIRECTORY}/observation_equations/codes
${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp
${THIRDPARTY_DIRECTORY}/Fusion/Fusion)

target_compile_definitions(mandeye_raw_data_viewer PRIVATE UTL_PROFILER_DISABLE)

target_link_libraries(
mandeye_raw_data_viewer
PRIVATE vqf
PRIVATE vqf Fusion
unordered_dense::unordered_dense
spdlog::spdlog
UTL::include
Expand Down
47 changes: 28 additions & 19 deletions core/include/imu_preintegration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,16 +30,16 @@ struct IntegrationParams

namespace imu_utils
{
Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g = 9.81);
Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg);
double safe_dt(double t_prev, double t_curr, double max_dt);
bool has_nan(const Eigen::Vector3d& v);
bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold);
std::vector<Eigen::Matrix3d> estimate_orientations(
const std::vector<RawIMUData>& raw_imu_data,
const Eigen::Matrix3d& initial_orientation,
const IntegrationParams& params,
const VQFParams& vqf_params = VQFParams());
Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g = 9.81);
Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg);
double safe_dt(double t_prev, double t_curr, double max_dt);
bool has_nan(const Eigen::Vector3d& v);
bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold);
std::vector<Eigen::Matrix3d> estimate_orientations(
const std::vector<RawIMUData>& raw_imu_data,
const Eigen::Matrix3d& initial_orientation,
const IntegrationParams& params,
const VQFParams& vqf_params = VQFParams());
} // namespace imu_utils

class AccelerationModel
Expand Down Expand Up @@ -128,15 +128,24 @@ inline const char* to_string(PreintegrationMethod method)
{
switch (method)
{
case PreintegrationMethod::euler_no_gravity_sm_vel: return "Euler, no gravity comp., SM velocity";
case PreintegrationMethod::trapezoidal_no_gravity_sm_vel: return "Trapezoidal, no gravity comp., SM velocity";
case PreintegrationMethod::euler_gravity_sm_vel: return "Euler, gravity comp., SM velocity";
case PreintegrationMethod::trapezoidal_gravity_sm_vel: return "Trapezoidal, gravity comp., SM velocity";
case PreintegrationMethod::kalman_gravity_sm_vel: return "Kalman, gravity comp., SM velocity";
case PreintegrationMethod::euler_gravity_ahrs_vel: return "Euler, gravity comp., AHRS velocity";
case PreintegrationMethod::trapezoidal_gravity_ahrs_vel: return "Trapezoidal, gravity comp., AHRS velocity";
case PreintegrationMethod::kalman_gravity_ahrs_vel: return "Kalman, gravity comp., AHRS velocity";
default: return "unknown";
case PreintegrationMethod::euler_no_gravity_sm_vel:
return "Euler, no gravity comp., SM velocity";
case PreintegrationMethod::trapezoidal_no_gravity_sm_vel:
return "Trapezoidal, no gravity comp., SM velocity";
case PreintegrationMethod::euler_gravity_sm_vel:
return "Euler, gravity comp., SM velocity";
case PreintegrationMethod::trapezoidal_gravity_sm_vel:
return "Trapezoidal, gravity comp., SM velocity";
case PreintegrationMethod::kalman_gravity_sm_vel:
return "Kalman, gravity comp., SM velocity";
case PreintegrationMethod::euler_gravity_ahrs_vel:
return "Euler, gravity comp., AHRS velocity";
case PreintegrationMethod::trapezoidal_gravity_ahrs_vel:
return "Trapezoidal, gravity comp., AHRS velocity";
case PreintegrationMethod::kalman_gravity_ahrs_vel:
return "Kalman, gravity comp., AHRS velocity";
default:
return "unknown";
}
}

Expand Down
Loading
Loading