diff --git a/include/modules/impl/lidar_odom_impl.hpp b/include/modules/impl/lidar_odom_impl.hpp index 4a0a534..2d8a35c 100644 --- a/include/modules/impl/lidar_odom_impl.hpp +++ b/include/modules/impl/lidar_odom_impl.hpp @@ -321,6 +321,9 @@ void LidarOdometry::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_, @@ -342,14 +345,14 @@ bool LidarOdometry::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; @@ -369,7 +372,8 @@ typename LidarOdometry::IterationStatus LidarOdometry::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 {}; } @@ -414,7 +418,10 @@ typename LidarOdometry::IterationStatus LidarOdometry::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(); @@ -508,6 +515,15 @@ bool LidarOdometry::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_) { @@ -710,7 +726,7 @@ void LidarOdometry::setInputSources() } template -void LidarOdometry::getNextPose(const std::optional& align_estimate) +bool LidarOdometry::getNextPose(const std::optional& align_estimate) { // // FRAME-TO-FRAME PROCEDURE @@ -732,16 +748,20 @@ void LidarOdometry::getNextPose(const std::optional& 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 @@ -768,10 +788,23 @@ void LidarOdometry::getNextPose(const std::optional& 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 @@ -779,13 +812,15 @@ void LidarOdometry::getNextPose(const std::optional& align_estimate) // Set next target cloud as current source cloud *this->target_cloud = *this->current_scan; + + return true; } template -void LidarOdometry::propagateS2S(const Mat4f& T) +void LidarOdometry::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 diff --git a/include/modules/imu_integrator.hpp b/include/modules/imu_integrator.hpp index 4246f8e..189dd16 100644 --- a/include/modules/imu_integrator.hpp +++ b/include/modules/imu_integrator.hpp @@ -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( @@ -121,6 +122,7 @@ class ImuIntegrator std::atomic use_orientation; const double calib_time; + double trim_window{600.0}; }; @@ -156,10 +158,13 @@ void ImuIntegrator::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)); + } } { @@ -179,10 +184,12 @@ void ImuIntegrator::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)); + } } } @@ -195,6 +202,14 @@ void ImuIntegrator::trimSamples(TimeFloatT trim_ts) util::tsq::trimToStamp(this->raw_buffer, trim_ts); } +template +void ImuIntegrator::setAutoTrimWindow(TimeFloatT window_s) +{ + std::unique_lock imu_lock{this->mtx}; + + this->trim_window = window_s; +} + template bool ImuIntegrator::recalibrate(TimeFloatT dt, bool force) { @@ -265,6 +280,8 @@ typename ImuIntegrator::Quat ImuIntegrator::getDelta( TimeFloatT start, TimeFloatT end) const { + using namespace util::geom::cvt::ops; + Quat q = Quat::Identity(); std::unique_lock imu_lock{this->mtx}; @@ -282,17 +299,17 @@ typename ImuIntegrator::Quat ImuIntegrator::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; @@ -300,54 +317,71 @@ typename ImuIntegrator::Quat ImuIntegrator::getDelta( } 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(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(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; diff --git a/include/modules/lidar_odom.hpp b/include/modules/lidar_odom.hpp index d4597bd..5abe304 100644 --- a/include/modules/lidar_odom.hpp +++ b/include/modules/lidar_odom.hpp @@ -151,7 +151,7 @@ class LidarOdometry void initializeInputTarget(); void setInputSources(); - void getNextPose(const std::optional& align_estimate = std::nullopt); + bool getNextPose(const std::optional& align_estimate = std::nullopt); void propagateS2S(const Mat4f& T); void propagateS2M(); @@ -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; diff --git a/include/modules/scan_preprocessor.hpp b/include/modules/scan_preprocessor.hpp index dbe8633..31fe15e 100644 --- a/include/modules/scan_preprocessor.hpp +++ b/include/modules/scan_preprocessor.hpp @@ -388,11 +388,20 @@ int ScanPreprocessor::filterPoints(const PointCloudMsg& scan) continue; } + bool out_was_nan = false; // loop through each zone tf for (const auto& zones_tf : this->computed_tf_zones) { + out_was_nan = false; // transform point to output out_pt_v3m = zones_tf.first * in_pt.getVector3fMap(); + if (out_pt_v3m.array().isNaN().any()) + { + // for some reason some points pass the input NaN test + // but blow up when being transformed??? + out_was_nan = true; + continue; + } bool br = false; // check all bounding zones in this frame @@ -414,6 +423,13 @@ int ScanPreprocessor::filterPoints(const PointCloudMsg& scan) break; } } + + if (out_was_nan) + { + out_pt_v3m.setZero(); + this->null_indices.push_back(i); + this->remove_indices.push_back(i); + } } if constexpr (!(Config_Value & PREPROC_PERFORM_DESKEW)) diff --git a/include/modules/ue_octree.hpp b/include/modules/ue_octree.hpp index 8168e2e..efcffbb 100644 --- a/include/modules/ue_octree.hpp +++ b/include/modules/ue_octree.hpp @@ -98,9 +98,6 @@ class UEOctree Node() = default; inline ~Node() = default; - void init(UEOctree*); - void clear(UEOctree*); - Node& operator[](size_t i); const Node& operator[](size_t i) const; @@ -117,6 +114,12 @@ class UEOctree }; protected: + Node* allocChildren(); + void initChildren(Node&); + void recursiveClearChildren(Node&); + void collapseAsExplored(Node&); + void collapseAsUnexplored(Node&); + static bool isLeaf(const NodeDescriptor& n); static Vec3i getDescriptorKey(const NodeDescriptor& n); static Vec3f getDescriptorKeyF(const NodeDescriptor& n); @@ -164,13 +167,13 @@ UEOctree::UEOctree(FloatT res) : vox_res{res} template UEOctree::~UEOctree() { - this->root.clear(this); + this->recursiveClearChildren(this->root); } template void UEOctree::clear() { - this->root.clear(this); + this->collapseAsUnexplored(this->root); this->root_height = 0; this->vox_span = 0; } @@ -335,42 +338,61 @@ typename UEOctree::FloatT UEOctree::distToUnexplored( template -void UEOctree::Node::init(UEOctree* inst) +typename UEOctree::Node& UEOctree::Node::operator[](size_t i) +{ + // assert(i < 8 && this->children); + return this->children[i]; +} +template +const typename UEOctree::Node& UEOctree::Node::operator[]( + size_t i) const +{ + // assert(i < 8 && this->children); + return this->children[i]; +} + + +template +typename UEOctree::Node* UEOctree::allocChildren() +{ + Node* children = new Node[8]; + this->alloc_estimate += sizeof(Node) * 8; + return children; +} +template +void UEOctree::initChildren(Node& node) { - if (this->isNull()) + if(node.isNull()) { - this->children = new Node[8]; - inst->alloc_estimate += sizeof(Node) * 8; + node.children = this->allocChildren(); } } template -void UEOctree::Node::clear(UEOctree* inst) +void UEOctree::recursiveClearChildren(Node& node) { - if (this->children) + if(node.children) { for(size_t i = 0; i < 8; i++) { - this->children[i].clear(inst); + this->recursiveClearChildren(node[i]); } - delete[] this->children; - this->children = nullptr; - inst->alloc_estimate -= sizeof(Node) * 8; + delete[] node.children; + node.children = nullptr; + this->alloc_estimate -= sizeof(Node) * 8; } } - template -typename UEOctree::Node& UEOctree::Node::operator[](size_t i) +void UEOctree::collapseAsExplored(Node& node) { - // assert(i < 8 && this->children); - return this->children[i]; + this->recursiveClearChildren(node); + node.explored = true; } template -const typename UEOctree::Node& UEOctree::Node::operator[]( - size_t i) const +void UEOctree::collapseAsUnexplored(Node& node) { - // assert(i < 8 && this->children); - return this->children[i]; + this->recursiveClearChildren(node); + node.explored = false; } @@ -481,7 +503,7 @@ bool UEOctree::adjustBounds(const Vec3f& min, const Vec3f& max) if (!this->root.anyExplored()) { this->origin = min; - FloatT max_diff = ((max - min) / this->vox_res).maxCoeff(); + const FloatT max_diff = ((max - min) / this->vox_res).maxCoeff(); this->root_height = static_cast(std::ceil(std::log2(std::max(max_diff, 1.f)))); this->vox_span = (0x1 << std::min(this->root_height, MAX_DEPTH)); @@ -501,9 +523,9 @@ bool UEOctree::adjustBounds(const Vec3f& min, const Vec3f& max) return false; } - size_t x_bit = min.x() < this->origin.x() ? 0x1 : 0x0; - size_t y_bit = min.y() < this->origin.y() ? 0x1 : 0x0; - size_t z_bit = min.z() < this->origin.z() ? 0x1 : 0x0; + const size_t x_bit = min.x() < this->origin.x() ? 0x1 : 0x0; + const size_t y_bit = min.y() < this->origin.y() ? 0x1 : 0x0; + const size_t z_bit = min.z() < this->origin.z() ? 0x1 : 0x0; const Vec3f coeffs{ static_cast(x_bit), static_cast(y_bit), @@ -513,8 +535,8 @@ bool UEOctree::adjustBounds(const Vec3f& min, const Vec3f& max) this->root_height++; if (this->root.anyExplored()) { - size_t octant = (x_bit << 2) | (y_bit << 1) | (z_bit << 0); - Node* tmp = new Node[8]; + const size_t octant = (x_bit << 2) | (y_bit << 1) | (z_bit << 0); + Node* tmp = this->allocChildren(); tmp[octant].children = root.children; tmp[octant].explored = root.explored; root.children = tmp; @@ -534,7 +556,7 @@ bool UEOctree::adjustBounds(const Vec3f& min, const Vec3f& max) this->root_height++; if (this->root.anyExplored()) { - Node* tmp = new Node[8]; + Node* tmp = this->allocChildren(); tmp[0].children = root.children; tmp[0].explored = root.explored; root.children = tmp; @@ -552,7 +574,7 @@ void UEOctree::recursiveExplore( const NodeDescriptor& descriptor, const Box3f& zone) { - node.init(this); + this->initChildren(node); bool all_explored = true; for (size_t i = 0; i < 8; i++) { @@ -562,19 +584,17 @@ void UEOctree::recursiveExplore( if (zone.contains(child_span)) { - child.explored = true; - child.clear(this); + this->collapseAsExplored(child); } else if (zone.intersects(child_span)) { - if (!isLeaf(child_desc)) + if (isLeaf(child_desc)) { - this->recursiveExplore(child, child_desc, zone); + this->collapseAsExplored(child); } else { - child.explored = true; - child.clear(this); + this->recursiveExplore(child, child_desc, zone); } } @@ -583,8 +603,7 @@ void UEOctree::recursiveExplore( if (all_explored) { - node.explored = true; - node.clear(this); + this->collapseAsExplored(node); } } @@ -604,7 +623,7 @@ void UEOctree::recursiveCrop( { // not leaf --> expand children and recurse const bool was_explored = node.explored; - node.init(this); + this->initChildren(node); node.explored = false; bool all_explored = true; @@ -624,8 +643,7 @@ void UEOctree::recursiveCrop( if (all_explored) { - node.explored = true; - node.clear(this); + this->collapseAsExplored(node); } } // leaf --> leave alone @@ -633,8 +651,7 @@ void UEOctree::recursiveCrop( else { // not contained and not intersected --> delete - node.explored = false; - node.clear(this); + this->collapseAsUnexplored(node); } } } diff --git a/include/util/cloud_ops.hpp b/include/util/cloud_ops.hpp index 395a7e7..c190109 100644 --- a/include/util/cloud_ops.hpp +++ b/include/util/cloud_ops.hpp @@ -913,7 +913,7 @@ void combineSortedSelections( } /** Remove the points at the each index in the provided set. - * Prereq: selection indices must be sorted in non-descending order! */ + * Prereq: selection must be valid for the point set and in increasing order! */ template< typename PointT = pcl::PointXYZ, typename AllocT = @@ -923,12 +923,10 @@ void removeSelection( std::vector& points, const std::vector& selection) { - ASSERT_POINT_HAS_XYZ(PointT) - // assert sizes size_t last = points.size() - 1; - for (int64_t i = static_cast(selection.size()) - 1; i >= 0; i--) + for (size_t i = selection.size(); i-- > 0;) { - points[selection[i]] = points[last]; + points[selection[i]] = std::move(points[last]); last--; } points.resize(last + 1); @@ -956,11 +954,9 @@ inline void trimToSelection( std::vector& points, const std::vector& selection) { - ASSERT_POINT_HAS_XYZ(PointT) - for (size_t i = 0; i < selection.size(); i++) { - points[i] = points[selection[i]]; + points[i] = std::move(points[selection[i]]); } points.resize(selection.size()); } @@ -987,8 +983,6 @@ inline void copySelection( const std::vector& selection, std::vector& buffer) { - // ASSERT_POINT_HAS_XYZ(PointT) - buffer.resize(selection.size()); for (size_t i = 0; i < selection.size(); i++) { @@ -1019,8 +1013,6 @@ void copyInverseSelection( const std::vector& selection, std::vector& buffer) { - ASSERT_POINT_HAS_XYZ(PointT) - buffer.resize(points.size() - selection.size()); size_t base = 0, select = 0, negate = 0; for (; base < points.size() && negate < buffer.size(); base++) diff --git a/src/core/threads/localization_worker.cpp b/src/core/threads/localization_worker.cpp index 96be4b4..fb2166d 100644 --- a/src/core/threads/localization_worker.cpp +++ b/src/core/threads/localization_worker.cpp @@ -76,7 +76,8 @@ using TwistStampedMsg = geometry_msgs::msg::TwistStamped; using TransformStampedMsg = geometry_msgs::msg::TransformStamped; using ReflectorHintMsg = cardinal_perception::msg::ReflectorHint; -using TrajectoryFilterDebugMsg = cardinal_perception::msg::TrajectoryFilterDebug; +using TrajectoryFilterDebugMsg = + cardinal_perception::msg::TrajectoryFilterDebug; namespace csm @@ -108,10 +109,7 @@ void LocalizationWorker::configure( this->map_frame = map_frame; this->odom_frame = odom_frame; this->base_frame = base_frame; - this->transform_sync.setFrameIds( - map_frame, - odom_frame, - base_frame); + this->transform_sync.setFrameIds(map_frame, odom_frame, base_frame); } void LocalizationWorker::accept(const PointCloudMsg::ConstSharedPtr& msg) @@ -122,7 +120,7 @@ void LocalizationWorker::accept(const PointCloudMsg::ConstSharedPtr& msg) #if TAG_DETECTION_ENABLED void LocalizationWorker::accept(const TagsTransformMsg::ConstSharedPtr& msg) { - if(!this->global_alignment_enabled) + if (!this->global_alignment_enabled) { return; } @@ -420,7 +418,7 @@ void LocalizationWorker::fiducial_thread_worker() break; } - if(!this->global_alignment_enabled) + if (!this->global_alignment_enabled) { continue; }