Skip to content

Battle hardened odometry #72

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
S1ink merged 1 commit into from
Jan 19, 2026
Merged

Battle hardened odometry #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
67 changes: 51 additions & 16 deletions include/modules/impl/lidar_odom_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -321,6 +321,9 @@ void LidarOdometry<PT>::initState()
this->gicp.setSearchMethodSource(temp, true);
this->gicp.setSearchMethodTarget(temp, true);

// this->gicp_s2s.setDebugPrint(true);
// this->gicp.setDebugPrint(true);

this->vf_scan.setLeafSize(
this->param.vf_scan_res_,
this->param.vf_scan_res_,
Expand All @@ -342,14 +345,14 @@ bool LidarOdometry<PT>::setInitial(const util::geom::Pose3f& pose)
// set known position
this->state.T.template block<3, 1>(0, 3) =
this->state.T_s2s.template block<3, 1>(0, 3) =
this->state.T_s2s_prev.template block<3, 1>(0, 3) =
this->state.T_prev.template block<3, 1>(0, 3) =
this->state.translation = pose.vec;

// set known orientation
this->state.last_rotq = this->state.rotq = pose.quat;
this->state.T.template block<3, 3>(0, 0) =
this->state.T_s2s.template block<3, 3>(0, 0) =
this->state.T_s2s_prev.template block<3, 3>(0, 0) =
this->state.T_prev.template block<3, 3>(0, 0) =
this->state.rotq.toRotationMatrix();

return true;
Expand All @@ -369,7 +372,8 @@ typename LidarOdometry<PT>::IterationStatus LidarOdometry<PT>::processScan(
if (stamp <= this->state.prev_frame_stamp)
{
std::cout << "[ODOMETRY]: Scan timestamp is older than the previous by "
<< (this->state.prev_frame_stamp - stamp) << "s!" << std::endl;
<< (this->state.prev_frame_stamp - stamp) << "s!"
<< std::endl;
return {};
}

Expand Down Expand Up @@ -414,7 +418,10 @@ typename LidarOdometry<PT>::IterationStatus LidarOdometry<PT>::processScan(
this->setInputSources();

// Get the next pose via IMU + S2S + S2M
this->getNextPose(align_estimate);
if(!this->getNextPose(align_estimate))
{
return {};
}

// Transform point cloud
this->transformCurrentScan();
Expand Down Expand Up @@ -508,6 +515,15 @@ bool LidarOdometry<PT>::preprocessPoints(const PointCloudT& scan)
return false;
}

// for(const auto& pt : scan.points)
// {
// if(pt.getArray3fMap().isNaN().any())
// {
// std::cout << "[ODOMETRY]: Input cloud had NaN input!" << std::endl;
// return false;
// }
// }

// Find new voxel size before applying filter
if (this->param.adaptive_params_use_)
{
Expand Down Expand Up @@ -710,7 +726,7 @@ void LidarOdometry<PT>::setInputSources()
}

template<typename PT>
void LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
bool LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
{
//
// FRAME-TO-FRAME PROCEDURE
Expand All @@ -732,16 +748,20 @@ void LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
}

// Get the local S2S transform
Mat4f T_S2S = this->gicp_s2s.getFinalTransformation();
const Mat4f T_s2s_local = this->gicp_s2s.getFinalTransformation();

if (T_s2s_local.array().isNaN().any())
{
std::cout << "[ODOMETRY]: S2S transform contained NaN values!"
<< std::endl;
return false;
}

// Get the global S2S transform
this->propagateS2S(T_S2S);
this->propagateS2S(T_s2s_local);

// reuse covariances from s2s for s2m
this->gicp.source_covs_ = this->gicp_s2s.source_covs_;

// Swap source and target (which also swaps KdTrees internally) for next S2S
this->gicp_s2s.swapSourceAndTarget();
this->gicp.source_covs_ = this->gicp_s2s.target_covs_;

//
// FRAME-TO-SUBMAP
Expand All @@ -768,24 +788,39 @@ void LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
}

// Get final transformation in global frame
this->state.T = this->gicp.getFinalTransformation();
const Mat4f T = this->gicp.getFinalTransformation();

if (T.array().isNaN().any())
{
std::cout << "[ODOMETRY]: S2M transform contained NaN values!"
<< std::endl;
return false;
}

this->state.T = std::move(T);

// Swap source and target (which also swaps KdTrees internally) for next S2S
// (don't do this until we know it is safe to do so)
this->gicp_s2s.swapSourceAndTarget();

// Update the S2S transform for next propagation
this->state.T_s2s_prev = this->state.T;
this->state.T_prev = this->state.T;

// Update next global pose
// Both source and target clouds are in the global frame now, so tranformation is global
this->propagateS2M();

// Set next target cloud as current source cloud
*this->target_cloud = *this->current_scan;

return true;
}

template<typename PT>
void LidarOdometry<PT>::propagateS2S(const Mat4f& T)
void LidarOdometry<PT>::propagateS2S(const Mat4f& T_rel)
{
this->state.T_s2s = this->state.T_s2s_prev * T;
this->state.T_s2s_prev = this->state.T_s2s;
this->state.T_s2s = this->state.T_prev * T_rel;
// this->state.T_prev = this->state.T_s2s;
}

template<typename PT>
Expand Down
142 changes: 88 additions & 54 deletions include/modules/imu_integrator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,7 @@ class ImuIntegrator
public:
void addSample(const ImuMsg& imu);
void trimSamples(TimeFloatT trim_ts);
void setAutoTrimWindow(TimeFloatT window_s = 0.f);
bool recalibrate(TimeFloatT dt, bool force = false);

Vec3 estimateGravity(
Expand Down Expand Up @@ -121,6 +122,7 @@ class ImuIntegrator
std::atomic<bool> use_orientation;

const double calib_time;
double trim_window{600.0};
};


Expand Down Expand Up @@ -156,10 +158,13 @@ void ImuIntegrator<F>::addSample(const ImuMsg& imu)
stamp,
q);

// 10 min max, orientation is likely still valid but at this point we probably have worse problems
util::tsq::trimToStamp(
this->orient_buffer,
(util::tsq::newestStamp(this->orient_buffer) - 600.));
if (this->trim_window > 0)
{
util::tsq::trimToStamp(
this->orient_buffer,
(util::tsq::newestStamp(this->orient_buffer) -
this->trim_window));
}
}

{
Expand All @@ -179,10 +184,12 @@ void ImuIntegrator<F>::addSample(const ImuMsg& imu)
this->recalibrateRange(0, this->raw_buffer.size());
}

// 5 min max, integration is definitely deviated after this for most imus
util::tsq::trimToStamp(
this->raw_buffer,
(util::tsq::newestStamp(this->raw_buffer) - 300.));
if (this->trim_window > 0)
{
util::tsq::trimToStamp(
this->raw_buffer,
(util::tsq::newestStamp(this->raw_buffer) - this->trim_window));
}
}
}

Expand All @@ -195,6 +202,14 @@ void ImuIntegrator<F>::trimSamples(TimeFloatT trim_ts)
util::tsq::trimToStamp(this->raw_buffer, trim_ts);
}

template<typename F>
void ImuIntegrator<F>::setAutoTrimWindow(TimeFloatT window_s)
{
std::unique_lock imu_lock{this->mtx};

this->trim_window = window_s;
}

template<typename F>
bool ImuIntegrator<F>::recalibrate(TimeFloatT dt, bool force)
{
Expand Down Expand Up @@ -265,6 +280,8 @@ typename ImuIntegrator<F>::Quat ImuIntegrator<F>::getDelta(
TimeFloatT start,
TimeFloatT end) const
{
using namespace util::geom::cvt::ops;

Quat q = Quat::Identity();
std::unique_lock imu_lock{this->mtx};

Expand All @@ -282,72 +299,89 @@ typename ImuIntegrator<F>::Quat ImuIntegrator<F>::getDelta(
newest--;
}

if (newest != oldest)
if (newest < oldest)
{
const auto& a = this->orient_buffer[oldest];
const auto& b = this->orient_buffer[oldest - 1];
const auto& c = this->orient_buffer[newest + 1];
const auto& d = this->orient_buffer[newest];

Quat prev = a.second.slerp(
const Quat prev = a.second.slerp(
(start - a.first) / (b.first - a.first),
b.second);
Quat curr =
const Quat curr =
c.second.slerp((end - c.first) / (d.first - c.first), d.second);

q = prev.inverse() * curr;
}
}
else
{
assert(!"IMU angular velocity integration is not implemented!");
#if 0 // TODO
const size_t
init_idx = util::tsq::binarySearchIdx(this->imu_buffer, start),
end_idx = util::tsq::binarySearchIdx(this->imu_buffer, end);

// Relative IMU integration of gyro and accelerometer
double curr_imu_stamp = 0.;
double prev_imu_stamp = 0.;
double dt;

for(size_t i = init_idx; i >= end_idx; i--)
size_t oldest = util::tsq::binarySearchIdx(this->raw_buffer, start);
size_t newest = util::tsq::binarySearchIdx(this->raw_buffer, end);

if (oldest == this->raw_buffer.size() && oldest > 0)
{
oldest--;
}
if (newest > 0)
{
const auto& imu_sample = this->raw_buffer[i];
newest--;
}

if(prev_imu_stamp == 0.)
if (newest < oldest)
{
const auto& a = this->raw_buffer[oldest];
const auto& b = this->raw_buffer[oldest - 1];
const auto& c = this->raw_buffer[newest + 1];
const auto& d = this->raw_buffer[newest];

const Vec3 head =
a.second.ang_vel + (b.second.ang_vel - a.second.ang_vel) *
(start - a.first) / (b.first - a.first);
const Vec3 tail =
c.second.ang_vel + (d.second.ang_vel - c.second.ang_vel) *
(end - c.first) / (d.first - c.first);

for (size_t i = oldest; i > newest; i--)
{
prev_imu_stamp = imu_sample.first;
continue;
const Vec3 *from, *to;
double from_t, to_t;
if (i == oldest)
{
from = &head;
from_t = start;
}
else
{
from = &(this->raw_buffer[i].second.ang_vel);
from_t = this->raw_buffer[i].first;
}

if (i - 1 == newest)
{
to = &tail;
to_t = end;
}
else
{
to = &(this->raw_buffer[i - 1].second.ang_vel);
to_t = this->raw_buffer[i - 1].first;
}

const Vec3 avg_w = (*from + *to) * 0.5f;
const FloatT dt = static_cast<FloatT>(to_t - from_t);
const Vec3 theta = avg_w * dt;
const FloatT angle = theta.norm();

if (angle > 1e-8)
{
const Vec3 axis = theta / angle;
q = (q * Quat{Eigen::AngleAxis<FloatT>(angle, axis)})
.normalized();
}
}

// Calculate difference in imu measurement times IN SECONDS
curr_imu_stamp = imu_sample.first;
dt = curr_imu_stamp - prev_imu_stamp;
prev_imu_stamp = curr_imu_stamp;

// Relative gyro propagation quaternion dynamics
Quatd qq = q;
q.w() -= 0.5 * dt *
(qq.x() * imu_sample.second.ang_vel.x()
+ qq.y() * imu_sample.second.ang_vel.y()
+ qq.z() * imu_sample.second.ang_vel.z() );
q.x() += 0.5 * dt *
(qq.w() * imu_sample.second.ang_vel.x()
- qq.z() * imu_sample.second.ang_vel.y()
+ qq.y() * imu_sample.second.ang_vel.z() );
q.y() += 0.5 * dt *
(qq.z() * imu_sample.second.ang_vel.x()
+ qq.w() * imu_sample.second.ang_vel.y()
- qq.x() * imu_sample.second.ang_vel.z() );
q.z() += 0.5 * dt *
(qq.x() * imu_sample.second.ang_vel.y()
- qq.y() * imu_sample.second.ang_vel.x()
+ qq.w() * imu_sample.second.ang_vel.z() );
}

q.normalize();
#endif
}

return q;
Expand Down
4 changes: 2 additions & 2 deletions include/modules/lidar_odom.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ class LidarOdometry
void initializeInputTarget();
void setInputSources();

void getNextPose(const std::optional<Mat4f>& align_estimate = std::nullopt);
bool getNextPose(const std::optional<Mat4f>& align_estimate = std::nullopt);

void propagateS2S(const Mat4f& T);
void propagateS2M();
Expand Down Expand Up @@ -211,7 +211,7 @@ class LidarOdometry

Mat4f T{Mat4f::Identity()};
Mat4f T_s2s{Mat4f::Identity()};
Mat4f T_s2s_prev{Mat4f::Identity()};
Mat4f T_prev{Mat4f::Identity()};

std::mutex mtx;
} state;
Expand Down
Loading