From 7a36f8bda2a2f63c3bb46b0dd19b1b18d8d91287 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 16 Mar 2018 08:19:30 +0100
Subject: [PATCH] Make origin_ptr_ a CaptureMotionPtr
---
src/processor_capture_holder.cpp | 2 +-
src/processor_motion.cpp | 2 +-
src/processor_motion.h | 6 +++---
src/sensors/sensor_diff_drive.cpp | 8 +++-----
src/test/gtest_constraint_IMU.cpp | 4 ++--
src/test/gtest_processor_IMU.cpp | 4 ++--
6 files changed, 12 insertions(+), 14 deletions(-)
diff --git a/src/processor_capture_holder.cpp b/src/processor_capture_holder.cpp
index cc34d1d2f..398f829aa 100644
--- a/src/processor_capture_holder.cpp
+++ b/src/processor_capture_holder.cpp
@@ -90,7 +90,7 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
// {
// // go to the previous motion capture
// if (capture_ptr == last_ptr_)
-// capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+// capture_ptr = origin_ptr_;
// else if (capture_ptr->getOriginFramePtr() == nullptr)
// return nullptr;
// else
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 63cf1e074..b9d6d3218 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -307,7 +307,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
{
// go to the previous motion capture
if (capture_ptr == last_ptr_)
- capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+ capture_ptr = origin_ptr_;
else if (capture_ptr->getOriginFramePtr() == nullptr)
return nullptr;
else
diff --git a/src/processor_motion.h b/src/processor_motion.h
index bc1246972..2c0c776ae 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -400,7 +400,7 @@ class ProcessorMotion : public ProcessorBase
CaptureMotionPtr getCaptureMotionContainingTimeStamp(const TimeStamp& _ts);
public:
- virtual CaptureBasePtr getOriginPtr();
+ virtual CaptureMotionPtr getOriginPtr();
virtual CaptureMotionPtr getLastPtr();
virtual CaptureMotionPtr getIncomingPtr();
@@ -412,7 +412,7 @@ class ProcessorMotion : public ProcessorBase
Size delta_size_; ///< the size of the deltas
Size delta_cov_size_; ///< the size of the delta covariances matrix
Size calib_size_; ///< size of the extra parameters (TBD in derived classes)
- CaptureBasePtr origin_ptr_;
+ CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
@@ -534,7 +534,7 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
);
}
-inline CaptureBasePtr ProcessorMotion::getOriginPtr()
+inline CaptureMotionPtr ProcessorMotion::getOriginPtr()
{
return origin_ptr_;
}
diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp
index 52e4177c7..0931b700c 100644
--- a/src/sensors/sensor_diff_drive.cpp
+++ b/src/sensors/sensor_diff_drive.cpp
@@ -103,24 +103,22 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
// problem->installSensor() return a SensorBasePtr.
//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
//{
-// std::shared_ptr<CaptureMotion> capture_ptr = std::static_pointer_cast<CaptureMotion>(_capture_ptr);
-
// if (intrinsics_.data_is_position_)
// {
-// Eigen::Vector2s data = capture_ptr->getData();
+// Eigen::Vector2s data = _capture_ptr->getData();
// // dt is set to one as we are dealing with wheel position
// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
// intrinsics_.separation_, 1);
-// capture_ptr->setData(data);
+// _capture_ptr->setData(data);
// Eigen::Matrix2s data_cov;
// data_cov << 0.00001, 0, 0, 0.00001; // Todo
// computeDataCov(data, data_cov);
-// capture_ptr->setDataCovariance(data_cov);
+// _capture_ptr->setDataCovariance(data_cov);
// }
// /// @todo tofix
diff --git a/src/test/gtest_constraint_IMU.cpp b/src/test/gtest_constraint_IMU.cpp
index 3accb792b..4c4c85369 100644
--- a/src/test/gtest_constraint_IMU.cpp
+++ b/src/test/gtest_constraint_IMU.cpp
@@ -979,9 +979,9 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
sensor_odo->process(capture_odo);
-// WOLF_TRACE("Jac calib: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getJacobianCalib().row(0));
+// WOLF_TRACE("Jac calib: ", processor_imu->getOriginPtr()->getJacobianCalib().row(0));
// WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getCalibrationPreint().transpose());
+// WOLF_TRACE("orig calib preint: ", processor_imu->getOriginPtr()->getCalibrationPreint().transpose());
//prepare next odometry measurement
quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index f8aac4c30..7d8e3e549 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -437,7 +437,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
problem->setPrior(x0, P0, t, 0.01);
- std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+ problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
@@ -493,7 +493,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6s bias; bias << abx,aby,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
- std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+ problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
--
GitLab