Skip to content

Fix tracker time increasing issue #78

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

Open
zwdoescode wants to merge 1 commit into
base: main
Choose a base branch
from
+48 −22
Open

Fix tracker time increasing issue #78

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
65 changes: 45 additions & 20 deletions libs/slam/async_slam/async_slam.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,8 +116,6 @@ void AsyncSlam::TrackResult(FrameId frameId, int64_t timestamp_ns, const odom::I
const sof::Images& images, const Isometry3T& delta) {
TRACE_EVENT ev = profiler_domain_.trace_event("AsyncSlam::TrackResult()", profiler_color_);

assert(track_data_.from_keyframe.matrix().allFinite());

// extract telemetry
{
std::shared_ptr<AsyncSlamLCTelemetry> async_slam_telemetry;
Expand All @@ -130,22 +128,28 @@ void AsyncSlam::TrackResult(FrameId frameId, int64_t timestamp_ns, const odom::I

bool is_keyframe = stat.keyframe;

// first frame
if (is_first_frame_) {
VO_ResetFrameData(frameId, timestamp_ns, track_data_);
tail_.Clear();
is_first_frame_ = false;
}
VOTrackData track_data_snapshot;
{
std::lock_guard pose_state_guard(pose_state_mutex_);
assert(track_data_.from_keyframe.matrix().allFinite());

if (is_first_frame_) {
VO_ResetFrameData(frameId, timestamp_ns, track_data_);
tail_.Clear();
is_first_frame_ = false;
}

VO_IncrementFrameData(frameId, timestamp_ns, delta, track_data_);
VO_IncrementFrameData(frameId, timestamp_ns, delta, track_data_);
track_data_snapshot = track_data_;
}

const bool has_images = std::any_of(images.begin(), images.end(), [](const auto& image) { return image != nullptr; });
if (is_keyframe && has_images) {
const auto vo_keyframe = std::make_shared<VOKeyframeInfo>(VOKeyframeInfo());
const Isometry3T current_pose = GetSlamPose();

vo_keyframe->vo_pose_at_this_frame = current_pose;
vo_keyframe->track_data = track_data_;
vo_keyframe->track_data = track_data_snapshot;
vo_keyframe->frame_data.frame_id = frameId;
vo_keyframe->frame_data.timestamp_ns = timestamp_ns;
vo_keyframe->frame_data.frame_information = FrameInformationString(images);
Expand Down Expand Up @@ -194,10 +198,16 @@ void AsyncSlam::TrackResult(FrameId frameId, int64_t timestamp_ns, const odom::I
if (tail_.UpdateTimeByOdometry(timestamp_ns, current_pose)) {
input_queue_.Push(vo_keyframe);
}
VO_ResetFrameData(frameId, timestamp_ns, track_data_);

{
std::lock_guard pose_state_guard(pose_state_mutex_);
VO_ResetFrameData(frameId, timestamp_ns, track_data_);
track_data_snapshot = track_data_;
}
}
if (options_.pose_for_frame_required) {
trajectory_[frameId] = track_data_;
std::lock_guard pose_state_guard(pose_state_mutex_);
trajectory_[frameId] = track_data_snapshot;
}

#ifdef USE_RERUN
Expand All @@ -211,13 +221,18 @@ void AsyncSlam::TrackResult(FrameId frameId, int64_t timestamp_ns, const odom::I
}

Isometry3T AsyncSlam::GetSlamPose() const {
std::lock_guard slam_guard(slam_mutex_);
Isometry3T from_keyframe = Isometry3T::Identity();
{
std::lock_guard pose_state_guard(pose_state_mutex_);
from_keyframe = track_data_.from_keyframe;
}

const auto may_be_tip = tail_.GetTip();
if (!may_be_tip) {
return Isometry3T::Identity();
}
const Isometry3T& tail_tip = may_be_tip->second;
Isometry3T slam_pose = tail_tip * track_data_.from_keyframe;
Isometry3T slam_pose = tail_tip * from_keyframe;

if (options_.planar_constraints) {
slam_pose.translation().y() = 0;
Expand All @@ -244,12 +259,16 @@ bool AsyncSlam::GetPoseForFrame(FrameId frameId, Isometry3T& pose) const {
return false;
}

auto it = trajectory_.find(frameId);
if (it == trajectory_.end()) {
SlamStderr("Pose not found for frame %zd.\n", static_cast<uint64_t>(frameId));
return false;
VOTrackData track_data;
{
std::lock_guard pose_state_guard(pose_state_mutex_);
auto it = trajectory_.find(frameId);
if (it == trajectory_.end()) {
SlamStderr("Pose not found for frame %zd.\n", static_cast<uint64_t>(frameId));
return false;
}
track_data = it->second;
}
auto& track_data = it->second;
assert(track_data.from_keyframe.matrix().allFinite());

Isometry3T m;
Expand All @@ -268,8 +287,14 @@ bool AsyncSlam::GetPosesForAllFrames(std::map<uint64_t, storage::Isometry3<float
return false;
}

std::map<FrameId, VOTrackData> trajectory_snapshot;
{
std::lock_guard pose_state_guard(pose_state_mutex_);
trajectory_snapshot = trajectory_;
}

std::lock_guard slam_guard(slam_mutex_);
for (auto it : trajectory_) {
for (const auto& it : trajectory_snapshot) {
auto& track_data = it.second;
uint64_t timestamp_ns = it.second.timestamp_ns;
assert(track_data.from_keyframe.matrix().allFinite());
Expand Down
5 changes: 3 additions & 2 deletions libs/slam/async_slam/async_slam.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,12 +119,13 @@ class AsyncSlam {
std::unique_ptr<LocalizerAndMapper> slam_; // should be protected by slam_mutex_
Tail tail_; // thread-safe
std::unique_ptr<ILoopClosureSolver> loop_closure_solver_; // should be accessed from slam thread only
std::map<FrameId, VOTrackData> trajectory_;
mutable std::mutex pose_state_mutex_;
std::map<FrameId, VOTrackData> trajectory_; // guarded by pose_state_mutex_

std::thread thread_;

bool is_first_frame_ = true;
VOTrackData track_data_;
VOTrackData track_data_; // guarded by pose_state_mutex_

// profiler
profiler::SLAMProfiler::DomainHelper profiler_domain_ = profiler::SLAMProfiler::DomainHelper("SLAM");
Expand Down
Loading