From 14de257509ac5a89b1c29885f215b6c832368064 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 26 May 2020 23:26:29 +0200
Subject: [PATCH] Cleanup
---
src/processor/processor_motion.cpp | 10 ++++------
src/sensor/sensor_diff_drive.cpp | 8 ++------
2 files changed, 6 insertions(+), 12 deletions(-)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f376f83aa..652a58d7e 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -761,17 +761,15 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
if (pack)
{
if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
- {
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+
processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
- }
+
else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+
processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
else
+
processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
}
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 03634eb09..afdc43e02 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -22,7 +22,7 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
unfixIntrinsics();
- std::cout << "Prior cov diag " << params_diff_drive_->prior_cov_diag.transpose() << std::endl;
+
if(params_diff_drive_->set_intrinsics_prior)
addPriorIntrinsics(getIntrinsic()->getState(), params_diff_drive_->prior_cov_diag.asDiagonal());
@@ -31,17 +31,13 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
double sigma_rev = 2*radians_per_tick;
Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
-// // 2. unmeasured lateral wheel slippage: sigma = 1mm
-// double sigma_slippage = 0.001;
-// Eigen::Vector3d noise_sigma; noise_sigma << sigma_rev, sigma_rev, sigma_slippage;
-
setNoiseStd(noise_sigma);
}
SensorDiffDrive::~SensorDiffDrive()
{
- // TODO Auto-generated destructor stub
+ // Auto-generated destructor stub
}
--
GitLab