Skip to content
Snippets Groups Projects
Commit 14de2575 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Cleanup

parent 6016894e
No related branches found
No related tags found
1 merge request!366Resolve "Complete state vector new data structure?"
Pipeline #5445 passed
......@@ -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;
}
......
......@@ -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
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment