Skip to content
Snippets Groups Projects

WIP: Resolve "Adapt to std::set and std::map instead of std::list in wolf nodes"

Merged WIP: Resolve "Adapt to std::set and std::map instead of std::list in wolf nodes"
1-adapt-to-std-set-and-std-map-instead-of-std-list-in-wolf-nodes into devel
Merged Joaquim Casals Buñuel requested to merge 1-adapt-to-std-set-and-std-map-instead-of-std-list-in-wolf-nodes into devel
Compare
and
1 file
+ 3
− 3
Compare changes
  • Side-by-side
  • Inline
src/subscriber_imu.cpp
+ 3
− 3
@@ -79,11 +79,11 @@ void SubscriberImu::publishBias() const
// Fill PoseStamped msg
geometry_msgs::Vector3 accel_bias_msg, gyro_bias_msg;
if (not sensor_ptr_->getProblem()->getTrajectory()->getLastKeyFrame() or
not sensor_ptr_->getProblem()->getTrajectory()->getLastKeyFrame()->getCaptureOf(sensor_ptr_))
if (not sensor_ptr_->getProblem()->getTrajectory()->getLastFrame() or
not sensor_ptr_->getProblem()->getTrajectory()->getLastFrame()->getCaptureOf(sensor_ptr_))
return;
Eigen::Vector6d bias = sensor_ptr_->getProblem()->getTrajectory()->getLastKeyFrame()->getCaptureOf(sensor_ptr_)->getStateBlock('I')->getState();
Eigen::Vector6d bias = sensor_ptr_->getProblem()->getTrajectory()->getLastFrame()->getCaptureOf(sensor_ptr_)->getStateBlock('I')->getState();
//Eigen::Vector3d accel_bias = //sensor_ptr_->getIntrinsic()->getState().head(3);
//Eigen::Vector3d gyro_bias = //sensor_ptr_->getIntrinsic()->getState().tail(3);