From a43ae098d8fce68188971829dc1e40d14f2d97fe Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Thu, 14 Mar 2019 15:42:03 +0100
Subject: [PATCH] Removed Ptr() suffix from getFrame, getCapture, etc...
---
hello_wolf/factor_bearing.h | 6 +-
hello_wolf/factor_range_bearing.h | 10 +-
hello_wolf/hello_wolf.cpp | 14 +-
hello_wolf/processor_range_bearing.cpp | 12 +-
include/base/association/association_node.h | 2 +-
include/base/capture/capture_base.h | 24 +-
include/base/capture/capture_motion.h | 4 +-
.../ceres_wrapper/cost_function_wrapper.h | 4 +-
.../local_parametrization_wrapper.h | 4 +-
include/base/ceres_wrapper/solver_manager.h | 2 +-
include/base/factor/factor_AHP.h | 40 +-
include/base/factor/factor_GPS_2D.h | 2 +-
.../base/factor/factor_GPS_pseudorange_2D.h | 12 +-
.../base/factor/factor_GPS_pseudorange_3D.h | 12 +-
include/base/factor/factor_IMU.h | 40 +-
.../base/factor/factor_autodiff_distance_3D.h | 4 +-
.../base/factor/factor_autodiff_trifocal.h | 44 +-
include/base/factor/factor_base.h | 14 +-
include/base/factor/factor_container.h | 20 +-
include/base/factor/factor_corner_2D.h | 18 +-
include/base/factor/factor_diff_drive.h | 20 +-
include/base/factor/factor_fix_bias.h | 6 +-
include/base/factor/factor_odom_2D.h | 8 +-
include/base/factor/factor_odom_3D.h | 28 +-
include/base/factor/factor_point_2D.h | 10 +-
include/base/factor/factor_point_to_line_2D.h | 12 +-
include/base/factor/factor_pose_2D.h | 4 +-
include/base/factor/factor_pose_3D.h | 4 +-
.../base/factor/factor_quaternion_absolute.h | 2 +-
.../base/factor/factor_relative_2D_analytic.h | 6 +-
include/base/feature/feature_base.h | 6 +-
include/base/frame_base.h | 16 +-
include/base/landmark/landmark_base.h | 12 +-
include/base/problem.h | 14 +-
include/base/processor/processor_base.h | 8 +-
include/base/processor/processor_motion.h | 14 +-
include/base/processor/processor_tracker.h | 12 +-
.../processor_tracker_feature_trifocal.h | 4 +-
include/base/sensor/sensor_GPS.h | 4 +-
include/base/sensor/sensor_base.h | 10 +-
include/base/sensor/sensor_camera.h | 4 +-
include/base/solver/solver_manager.h | 2 +-
.../solver_suitesparse/ccolamd_ordering.h | 4 +-
include/base/solver_suitesparse/qr_solver.h | 16 +-
include/base/state_block.h | 4 +-
include/base/track_matrix.h | 4 +-
include/base/trajectory_base.h | 10 +-
src/association/association_node.cpp | 2 +-
src/association/association_tree.cpp | 4 +-
src/capture/capture_base.cpp | 12 +-
src/capture/capture_laser_2D.cpp | 2 +-
src/capture/capture_wheel_joint_position.cpp | 2 +-
src/ceres_wrapper/ceres_manager.cpp | 48 +-
src/ceres_wrapper/qr_manager.cpp | 2 +-
src/ceres_wrapper/solver_manager.cpp | 2 +-
src/examples/solver/test_iQR.cpp | 4 +-
src/examples/solver/test_iQR_wolf.cpp | 4 +-
src/examples/solver/test_iQR_wolf2.cpp | 30 +-
src/examples/test_2_lasers_offline.cpp | 16 +-
src/examples/test_analytic_odom_factor.cpp | 12 +-
src/examples/test_autodiff.cpp | 46 +-
src/examples/test_ceres_2_lasers.cpp | 18 +-
.../test_ceres_2_lasers_polylines.cpp | 6 +-
src/examples/test_diff_drive.cpp | 14 +-
src/examples/test_factor_AHP.cpp | 28 +-
src/examples/test_factor_imu.cpp | 98 ++--
src/examples/test_faramotics_simulation.cpp | 2 +-
src/examples/test_image.cpp | 6 +-
src/examples/test_imuDock.cpp | 48 +-
src/examples/test_imuDock_autoKFs.cpp | 16 +-
src/examples/test_imuPlateform_Offline.cpp | 38 +-
src/examples/test_imu_constrained0.cpp | 62 +-
src/examples/test_kf_callback.cpp | 4 +-
src/examples/test_map_yaml.cpp | 18 +-
src/examples/test_mpu.cpp | 24 +-
src/examples/test_processor_imu.cpp | 32 +-
src/examples/test_processor_imu_jacobians.cpp | 2 +-
src/examples/test_processor_odom_3D.cpp | 2 +-
.../test_processor_tracker_landmark.cpp | 12 +-
.../test_processor_tracker_landmark_image.cpp | 14 +-
src/examples/test_sort_keyframes.cpp | 2 +-
src/examples/test_sparsification.cpp | 6 +-
src/examples/test_state_quaternion.cpp | 6 +-
src/examples/test_virtual_hierarchy.cpp | 2 +-
src/examples/test_wolf_autodiffwrapper.cpp | 18 +-
src/examples/test_wolf_factories.cpp | 6 +-
src/examples/test_wolf_imported_graph.cpp | 64 +--
src/examples/test_wolf_prunning.cpp | 90 +--
src/factor/factor_base.cpp | 10 +-
src/feature/feature_base.cpp | 4 +-
src/frame_base.cpp | 32 +-
src/landmark/landmark_AHP.cpp | 12 +-
src/landmark/landmark_base.cpp | 12 +-
src/landmark/landmark_container.cpp | 36 +-
src/landmark/landmark_polyline_2D.cpp | 8 +-
src/problem.cpp | 212 +++----
src/processor/processor_IMU.cpp | 2 +-
src/processor/processor_base.cpp | 2 +-
src/processor/processor_capture_holder.cpp | 8 +-
src/processor/processor_diff_drive.cpp | 6 +-
...rocessor_frame_nearest_neighbor_filter.cpp | 30 +-
src/processor/processor_motion.cpp | 48 +-
src/processor/processor_odom_2D.cpp | 8 +-
src/processor/processor_odom_3D.cpp | 4 +-
src/processor/processor_tracker.cpp | 16 +-
src/processor/processor_tracker_feature.cpp | 4 +-
.../processor_tracker_feature_corner.cpp | 14 +-
.../processor_tracker_feature_trifocal.cpp | 2 +-
src/processor/processor_tracker_landmark.cpp | 6 +-
.../processor_tracker_landmark_corner.cpp | 42 +-
.../processor_tracker_landmark_image.cpp | 38 +-
.../processor_tracker_landmark_polyline.cpp | 30 +-
src/sensor/sensor_GPS.cpp | 4 +-
src/sensor/sensor_GPS_fix.cpp | 2 +-
src/sensor/sensor_base.cpp | 10 +-
src/sensor/sensor_camera.cpp | 2 +-
src/sensor/sensor_diff_drive.cpp | 2 +-
src/solver/solver_manager.cpp | 2 +-
src/solver_suitesparse/solver_manager.cpp | 16 +-
src/track_matrix.cpp | 10 +-
src/trajectory_base.cpp | 6 +-
test/gtest_IMU.cpp | 24 +-
test/gtest_capture_base.cpp | 36 +-
test/gtest_ceres_manager.cpp | 30 +-
test/gtest_factor_IMU.cpp | 542 +++++++++---------
test/gtest_factor_absolute.cpp | 16 +-
test/gtest_factor_autodiff.cpp | 34 +-
test/gtest_factor_autodiff_distance_3D.cpp | 2 +-
test/gtest_factor_autodiff_trifocal.cpp | 330 +++++------
test/gtest_feature_IMU.cpp | 28 +-
test/gtest_frame_base.cpp | 24 +-
test/gtest_odom_2D.cpp | 6 +-
test/gtest_param_prior.cpp | 16 +-
test/gtest_problem.cpp | 42 +-
test/gtest_processor_IMU.cpp | 42 +-
test/gtest_processor_base.cpp | 2 +-
...essor_frame_nearest_neighbor_filter_2D.cpp | 22 +-
...est_processor_tracker_feature_trifocal.cpp | 4 +-
test/gtest_sensor_camera.cpp | 6 +-
test/gtest_solver_manager.cpp | 8 +-
test/gtest_track_matrix.cpp | 2 +-
test/gtest_trajectory.cpp | 44 +-
wolf_scripts/substitution.sh | 2 +-
143 files changed, 1629 insertions(+), 1629 deletions(-)
diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 543dd98c3..ce6bb49fe 100644
--- a/hello_wolf/factor_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -24,9 +24,9 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", nullptr, nullptr, nullptr,
_landmark_other_ptr, _processor_ptr,
_apply_loss_function, _status,
- getCapturePtr()->getFramePtr()->getPPtr(),
- getCapturePtr()->getFramePtr()->getOPtr(),
- _landmark_other_ptr->getPPtr())
+ getCapture()->getFrame()->getP(),
+ getCapture()->getFrame()->getO(),
+ _landmark_other_ptr->getP())
{
//
}
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index 7f15eba9f..727e75a4e 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -34,11 +34,11 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
_processor_ptr, // processor having created this
_apply_loss_function, // apply loss function to residual?
_status, // active factor?
- _capture_own->getFramePtr()->getPPtr(), // robot position
- _capture_own->getFramePtr()->getOPtr(), // robot orientation state block
- _capture_own->getSensorPtr()->getPPtr(), // sensor position state block
- _capture_own->getSensorPtr()->getOPtr(), // sensor orientation state block
- _landmark_other_ptr->getPPtr()) // landmark position state block
+ _capture_own->getFrame()->getP(), // robot position
+ _capture_own->getFrame()->getO(), // robot orientation state block
+ _capture_own->getSensor()->getP(), // sensor position state block
+ _capture_own->getSensor()->getO(), // sensor orientation state block
+ _landmark_other_ptr->getP()) // landmark position state block
{
//
}
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 67bc01617..f7c8d1c0d 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -141,11 +141,11 @@ int main()
// NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
- sensor_rb->getOPtr()->unfix();
+ sensor_rb->getO()->unfix();
// NOTE: SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
- // sensor_rb->getPPtr()->unfix();
+ // sensor_rb->getP()->unfix();
// CONFIGURE ==========================================================
@@ -220,16 +220,16 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto sen : problem->getHardwarePtr()->getSensorList())
+ for (auto sen : problem->getHardware()->getSensorList())
for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto kf : problem->getTrajectoryPtr()->getFrameList())
+ for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
for (auto sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMapPtr()->getLandmarkList())
+ for (auto lmk : problem->getMap()->getLandmarkList())
for (auto sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
@@ -244,10 +244,10 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectoryPtr()->getFrameList())
+ for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
- for (auto lmk : problem->getMapPtr()->getLandmarkList())
+ for (auto lmk : problem->getMap()->getLandmarkList())
WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
std::cout << std::endl;
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 2d44189b5..cb72112ee 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -17,7 +17,7 @@ namespace wolf
ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) :
ProcessorBase("RANGE BEARING", _params)
{
- H_r_s = transform(_sensor_ptr->getPPtr()->getState(), _sensor_ptr->getOPtr()->getState());
+ H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState());
}
void ProcessorRangeBearing::process(CaptureBasePtr _capture)
@@ -64,15 +64,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
{
// known landmarks : recover landmark
lmk = std::static_pointer_cast<LandmarkPoint2D>(lmk_it->second);
- WOLF_TRACE("known lmk(", id, "): ", lmk->getPPtr()->getState().transpose());
+ WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
}
else
{
// new landmark:
// - create landmark
lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
- WOLF_TRACE("new lmk(", id, "): ", lmk->getPPtr()->getState().transpose());
- getProblem()->getMapPtr()->addLandmark(lmk);
+ WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
+ getProblem()->getMap()->addLandmark(lmk);
// - add to known landmarks
known_lmks.emplace(id, lmk);
}
@@ -80,7 +80,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
// 5. create feature
Vector2s rb(range,bearing);
auto ftr = std::make_shared<FeatureRangeBearing>(rb,
- getSensorPtr()->getNoiseCov());
+ getSensor()->getNoiseCov());
capture_rb->addFeature(ftr);
// 6. create factor
@@ -141,7 +141,7 @@ Eigen::Vector2s ProcessorRangeBearing::fromSensor(const Eigen::Vector2s& lmk_s)
Eigen::Vector2s ProcessorRangeBearing::toSensor(const Eigen::Vector2s& lmk_w) const
{
-// Trf H_w_r = transform(getSensorPtr()->getPPtr()->getState(), getSensorPtr()->getOPtr()->getState());
+// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
Trf H_w_r = transform(getProblem()->getCurrentState());
return (H_w_r * H_r_s).inverse() * lmk_w;
}
diff --git a/include/base/association/association_node.h b/include/base/association/association_node.h
index 21a10d480..c652ba6c4 100644
--- a/include/base/association/association_node.h
+++ b/include/base/association/association_node.h
@@ -102,7 +102,7 @@ class AssociationNode
* Returns a copy of up_node_ptr_
*
**/
- AssociationNode * upNodePtr() const;
+ AssociationNode * upNode() const;
/** \brief Computes node probability
*
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index b25996cb2..f9e4abe9e 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -56,7 +56,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
- FrameBasePtr getFramePtr() const;
+ FrameBasePtr getFrame() const;
void setFramePtr(const FrameBasePtr _frm_ptr);
void unlinkFromFrame(){frame_ptr_.reset();}
@@ -68,7 +68,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void getFactorList(FactorBasePtrList& _ctr_list);
- SensorBasePtr getSensorPtr() const;
+ SensorBasePtr getSensor() const;
virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
// constrained by
@@ -82,9 +82,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
StateBlockPtr getStateBlockPtr(unsigned int _i) const;
void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
- StateBlockPtr getSensorPPtr() const;
- StateBlockPtr getSensorOPtr() const;
- StateBlockPtr getSensorIntrinsicPtr() const;
+ StateBlockPtr getSensorP() const;
+ StateBlockPtr getSensorO() const;
+ StateBlockPtr getSensorIntrinsic() const;
void removeStateBlocks();
virtual void registerNewStateBlocks();
@@ -148,17 +148,17 @@ inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _
state_block_vec_[_i] = _sb_ptr;
}
-inline StateBlockPtr CaptureBase::getSensorPPtr() const
+inline StateBlockPtr CaptureBase::getSensorP() const
{
return getStateBlockPtr(0);
}
-inline StateBlockPtr CaptureBase::getSensorOPtr() const
+inline StateBlockPtr CaptureBase::getSensorO() const
{
return getStateBlockPtr(1);
}
-inline StateBlockPtr CaptureBase::getSensorIntrinsicPtr() const
+inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
return getStateBlockPtr(2);
}
@@ -168,7 +168,7 @@ inline unsigned int CaptureBase::id()
return capture_id_;
}
-inline FrameBasePtr CaptureBase::getFramePtr() const
+inline FrameBasePtr CaptureBase::getFrame() const
{
return frame_ptr_.lock();
}
@@ -198,7 +198,7 @@ inline TimeStamp CaptureBase::getTimeStamp() const
return time_stamp_;
}
-inline SensorBasePtr CaptureBase::getSensorPtr() const
+inline SensorBasePtr CaptureBase::getSensor() const
{
return sensor_ptr_.lock();
}
@@ -220,9 +220,9 @@ inline void CaptureBase::setTimeStampToNow()
inline bool CaptureBase::process()
{
- assert (getSensorPtr() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+ assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
- return getSensorPtr()->process(shared_from_this());
+ return getSensor()->process(shared_from_this());
}
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index a93b627aa..ab932da22 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -96,7 +96,7 @@ class CaptureMotion : public CaptureBase
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error);
// Origin frame
- FrameBasePtr getOriginFramePtr();
+ FrameBasePtr getOriginFrame();
void setOriginFramePtr(FrameBasePtr _frame_ptr);
// member data:
@@ -156,7 +156,7 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const
return _delta + _delta_error;
}
-inline FrameBasePtr CaptureMotion::getOriginFramePtr()
+inline FrameBasePtr CaptureMotion::getOriginFrame()
{
return origin_frame_ptr_.lock();
}
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
index d74804e0c..4a3f42dcb 100644
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ b/include/base/ceres_wrapper/cost_function_wrapper.h
@@ -29,7 +29,7 @@ class CostFunctionWrapper : public ceres::CostFunction
virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const;
- FactorBasePtr getFactorPtr() const;
+ FactorBasePtr getFactor() const;
};
inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) :
@@ -49,7 +49,7 @@ inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, doub
return factor_ptr_->evaluate(parameters, residuals, jacobians);
}
-inline FactorBasePtr CostFunctionWrapper::getFactorPtr() const
+inline FactorBasePtr CostFunctionWrapper::getFactor() const
{
return factor_ptr_;
}
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
index 4ae4182cc..fc046a7ec 100644
--- a/include/base/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/base/ceres_wrapper/local_parametrization_wrapper.h
@@ -31,7 +31,7 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
virtual int LocalSize() const;
- LocalParametrizationBasePtr getLocalParametrizationPtr() const;
+ LocalParametrizationBasePtr getLocalParametrization() const;
};
using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
@@ -57,7 +57,7 @@ inline int LocalParametrizationWrapper::LocalSize() const
return local_parametrization_ptr_->getLocalSize();
}
-inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrizationPtr() const
+inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrization() const
{
return local_parametrization_ptr_;
}
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
index 672de770f..10a97fb6e 100644
--- a/include/base/ceres_wrapper/solver_manager.h
+++ b/include/base/ceres_wrapper/solver_manager.h
@@ -44,7 +44,7 @@ class SolverManager
virtual void update();
- virtual ProblemPtr getProblemPtr();
+ virtual ProblemPtr getProblem();
private:
diff --git a/include/base/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
index 0c5df6764..33a1de8e2 100644
--- a/include/base/factor/factor_AHP.h
+++ b/include/base/factor/factor_AHP.h
@@ -73,30 +73,30 @@ inline FactorAHP::FactorAHP(const FeatureBasePtr& _ftr_ptr,
_processor_ptr,
_apply_loss_function,
_status,
- _ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr(),
- _ftr_ptr->getCapturePtr()->getFramePtr()->getOPtr(),
- _landmark_ptr->getAnchorFrame()->getPPtr(),
- _landmark_ptr->getAnchorFrame()->getOPtr(),
- _landmark_ptr->getPPtr()),
- anchor_sensor_extrinsics_p_(_ftr_ptr->getCapturePtr()->getSensorPPtr()->getState()),
- anchor_sensor_extrinsics_o_(_ftr_ptr->getCapturePtr()->getSensorOPtr()->getState()),
- intrinsic_(_ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr()->getState())
+ _ftr_ptr->getCapture()->getFrame()->getP(),
+ _ftr_ptr->getCapture()->getFrame()->getO(),
+ _landmark_ptr->getAnchorFrame()->getP(),
+ _landmark_ptr->getAnchorFrame()->getO(),
+ _landmark_ptr->getP()),
+ anchor_sensor_extrinsics_p_(_ftr_ptr->getCapture()->getSensorP()->getState()),
+ anchor_sensor_extrinsics_o_(_ftr_ptr->getCapture()->getSensorO()->getState()),
+ intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState())
{
// obtain some intrinsics from provided sensor
- distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getDistortionVector();
+ distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
}
inline Eigen::VectorXs FactorAHP::expectation() const
{
- FrameBasePtr frm_current = getFeaturePtr()->getCapturePtr()->getFramePtr();
- FrameBasePtr frm_anchor = getFrameOtherPtr();
- LandmarkBasePtr lmk = getLandmarkOtherPtr();
+ FrameBasePtr frm_current = getFeature()->getCapture()->getFrame();
+ FrameBasePtr frm_anchor = getFrameOther();
+ LandmarkBasePtr lmk = getLandmarkOther();
- const Eigen::MatrixXs frame_current_pos = frm_current->getPPtr()->getState();
- const Eigen::MatrixXs frame_current_ori = frm_current->getOPtr()->getState();
- const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getPPtr()->getState();
- const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getOPtr()->getState();
- const Eigen::MatrixXs lmk_pos_hmg = lmk ->getPPtr()->getState();
+ const Eigen::MatrixXs frame_current_pos = frm_current->getP()->getState();
+ const Eigen::MatrixXs frame_current_ori = frm_current->getO()->getState();
+ const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getP()->getState();
+ const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getO()->getState();
+ const Eigen::MatrixXs lmk_pos_hmg = lmk ->getP()->getState();
Eigen::Vector2s exp;
expectation(frame_current_pos.data(), frame_current_ori.data(), frame_anchor_pos.data(),
@@ -136,9 +136,9 @@ inline void FactorAHP::expectation(const T* const _current_frame_p,
TransformType T_R0_C0 = t_r0_c0 * q_r0_c0;
// current robot to current camera transform
- CaptureBasePtr current_capture = this->getFeaturePtr()->getCapturePtr();
- Translation<T, 3> t_r1_c1 (current_capture->getSensorPPtr()->getState().cast<T>());
- Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorOPtr()->getState()));
+ CaptureBasePtr current_capture = this->getFeature()->getCapture();
+ Translation<T, 3> t_r1_c1 (current_capture->getSensorP()->getState().cast<T>());
+ Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorO()->getState()));
Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
TransformType T_R1_C1 = t_r1_c1 * q_r1_c1;
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
index 503f8e1f8..b34e7c06e 100644
--- a/include/base/factor/factor_GPS_2D.h
+++ b/include/base/factor/factor_GPS_2D.h
@@ -16,7 +16,7 @@ class FactorGPS2D : public FactorAutodiff<FactorGPS2D, 2, 2>
public:
FactorGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
- FactorAutodiff<FactorGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
+ FactorAutodiff<FactorGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP())
{
//
}
diff --git a/include/base/factor/factor_GPS_pseudorange_2D.h b/include/base/factor/factor_GPS_pseudorange_2D.h
index 246399a92..40b8a5115 100644
--- a/include/base/factor/factor_GPS_pseudorange_2D.h
+++ b/include/base/factor/factor_GPS_pseudorange_2D.h
@@ -38,13 +38,13 @@ class FactorGPSPseudorange2D : public FactorAutodiff<FactorGPSPseudorange2D, 1,
_pr_ptr,
_apply_loss_function,
_status,
- _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to the initial pos frame
- _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame
- _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to the vehicle frame
+ _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to the initial pos frame
+ _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame
+ _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to the vehicle frame
// orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapPPtr()), // map position with respect to ecef
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapOPtr())) // map orientation with respect to ecef
+ _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapP()), // map position with respect to ecef
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapO())) // map orientation with respect to ecef
{
sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
diff --git a/include/base/factor/factor_GPS_pseudorange_3D.h b/include/base/factor/factor_GPS_pseudorange_3D.h
index 1132ad8a1..d7a33681c 100644
--- a/include/base/factor/factor_GPS_pseudorange_3D.h
+++ b/include/base/factor/factor_GPS_pseudorange_3D.h
@@ -31,13 +31,13 @@ class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3
FactorStatus _status = CTR_ACTIVE) :
FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
- _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
- _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
- _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame
+ _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to map frame
+ _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame wrt map frame
+ _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to base frame
// orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getMapPPtr(), // initial vehicle position wrt ecef frame
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getMapOPtr()) // initial vehicle orientation wrt ecef frame
+ _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapP(), // initial vehicle position wrt ecef frame
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapO()) // initial vehicle orientation wrt ecef frame
{
sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
diff --git a/include/base/factor/factor_IMU.h b/include/base/factor/factor_IMU.h
index b0e85f859..c1b9ba65a 100644
--- a/include/base/factor/factor_IMU.h
+++ b/include/base/factor/factor_IMU.h
@@ -147,21 +147,21 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
FactorStatus _status) :
FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
"IMU",
- _cap_origin_ptr->getFramePtr(),
+ _cap_origin_ptr->getFrame(),
_cap_origin_ptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
- _cap_origin_ptr->getFramePtr()->getPPtr(),
- _cap_origin_ptr->getFramePtr()->getOPtr(),
- _cap_origin_ptr->getFramePtr()->getVPtr(),
- _cap_origin_ptr->getSensorIntrinsicPtr(),
- _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr(),
- _ftr_ptr->getFramePtr()->getVPtr(),
- _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
+ _cap_origin_ptr->getFrame()->getP(),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_origin_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getCapture()->getSensorIntrinsic()),
dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
dq_preint_(_ftr_ptr->getMeasurement().data()+3),
dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
@@ -172,9 +172,9 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFramePtr()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getWbRateStdev()),
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
+ ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
+ wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
sqrt_A_r_dt_inv((Eigen::Matrix3s::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
{
@@ -393,18 +393,18 @@ inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
inline Eigen::VectorXs FactorIMU::expectation() const
{
- FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
- FrameBasePtr frm_previous = getFrameOtherPtr();
+ FrameBasePtr frm_current = getFeature()->getFrame();
+ FrameBasePtr frm_previous = getFrameOther();
//get information on current_frame in the FactorIMU
- const Eigen::Vector3s frame_current_pos = (frm_current->getPPtr()->getState());
- const Eigen::Quaternions frame_current_ori (frm_current->getOPtr()->getState().data());
- const Eigen::Vector3s frame_current_vel = (frm_current->getVPtr()->getState());
+ const Eigen::Vector3s frame_current_pos = (frm_current->getP()->getState());
+ const Eigen::Quaternions frame_current_ori (frm_current->getO()->getState().data());
+ const Eigen::Vector3s frame_current_vel = (frm_current->getV()->getState());
// get info on previous frame in the FactorIMU
- const Eigen::Vector3s frame_previous_pos = (frm_previous->getPPtr()->getState());
- const Eigen::Quaternions frame_previous_ori (frm_previous->getOPtr()->getState().data());
- const Eigen::Vector3s frame_previous_vel = (frm_previous->getVPtr()->getState());
+ const Eigen::Vector3s frame_previous_pos = (frm_previous->getP()->getState());
+ const Eigen::Quaternions frame_previous_ori (frm_previous->getO()->getState().data());
+ const Eigen::Vector3s frame_previous_vel = (frm_previous->getV()->getState());
// Define results vector and Map bits over it
Eigen::Matrix<Scalar, 10, 1> exp;
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
index 862107f21..bc957f3cf 100644
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -31,8 +31,8 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
_processor_ptr,
_apply_loss_function,
_status,
- _feat->getFramePtr()->getPPtr(),
- _frm_other->getPPtr())
+ _feat->getFrame()->getP(),
+ _frm_other->getP())
{
setFeaturePtr(_feat);
}
diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
index c4a806edc..5e8c59e2b 100644
--- a/include/base/factor/factor_autodiff_trifocal.h
+++ b/include/base/factor/factor_autodiff_trifocal.h
@@ -33,7 +33,7 @@ class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3,
*/
virtual ~FactorAutodiffTrifocal();
- FeatureBasePtr getFeaturePrevPtr();
+ FeatureBasePtr getFeaturePrev();
const Vector3s& getPixelCanonicalLast() const
{
@@ -140,16 +140,16 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(
_processor_ptr,
_apply_loss_function,
_status,
- _feature_prev_ptr->getFramePtr()->getPPtr(),
- _feature_prev_ptr->getFramePtr()->getOPtr(),
- _feature_origin_ptr->getFramePtr()->getPPtr(),
- _feature_origin_ptr->getFramePtr()->getOPtr(),
- _feature_last_ptr->getFramePtr()->getPPtr(),
- _feature_last_ptr->getFramePtr()->getOPtr(),
- _feature_last_ptr->getCapturePtr()->getSensorPPtr(),
- _feature_last_ptr->getCapturePtr()->getSensorOPtr() ),
+ _feature_prev_ptr->getFrame()->getP(),
+ _feature_prev_ptr->getFrame()->getO(),
+ _feature_origin_ptr->getFrame()->getP(),
+ _feature_origin_ptr->getFrame()->getO(),
+ _feature_last_ptr->getFrame()->getP(),
+ _feature_last_ptr->getFrame()->getO(),
+ _feature_last_ptr->getCapture()->getSensorP(),
+ _feature_last_ptr->getCapture()->getSensorO() ),
feature_prev_ptr_(_feature_prev_ptr),
- camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensorPtr())),
+ camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
sqrt_information_upper(Matrix3s::Zero())
{
setFeaturePtr(_feature_last_ptr);
@@ -160,14 +160,14 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(
Matrix<Scalar,3,2> J_m_u = K_inv.block(0,0,3,2);
// extract relevant states
- Vector3s wtr1 = _feature_prev_ptr ->getFramePtr() ->getPPtr() ->getState();
- Quaternions wqr1 = Quaternions(_feature_prev_ptr ->getFramePtr() ->getOPtr() ->getState().data() );
- Vector3s wtr2 = _feature_origin_ptr->getFramePtr() ->getPPtr() ->getState();
- Quaternions wqr2 = Quaternions(_feature_origin_ptr->getFramePtr() ->getOPtr() ->getState().data() );
- Vector3s wtr3 = _feature_last_ptr ->getFramePtr() ->getPPtr() ->getState();
- Quaternions wqr3 = Quaternions(_feature_last_ptr ->getFramePtr() ->getOPtr() ->getState().data() );
- Vector3s rtc = _feature_last_ptr ->getCapturePtr()->getSensorPPtr()->getState();
- Quaternions rqc = Quaternions(_feature_last_ptr ->getCapturePtr()->getSensorOPtr()->getState().data() );
+ Vector3s wtr1 = _feature_prev_ptr ->getFrame() ->getP() ->getState();
+ Quaternions wqr1 = Quaternions(_feature_prev_ptr ->getFrame() ->getO() ->getState().data() );
+ Vector3s wtr2 = _feature_origin_ptr->getFrame() ->getP() ->getState();
+ Quaternions wqr2 = Quaternions(_feature_origin_ptr->getFrame() ->getO() ->getState().data() );
+ Vector3s wtr3 = _feature_last_ptr ->getFrame() ->getP() ->getState();
+ Quaternions wqr3 = Quaternions(_feature_last_ptr ->getFrame() ->getO() ->getState().data() );
+ Vector3s rtc = _feature_last_ptr ->getCapture()->getSensorP()->getState();
+ Quaternions rqc = Quaternions(_feature_last_ptr ->getCapture()->getSensorO()->getState().data() );
// expectation // canonical units
vision_utils::TrifocalTensorBase<Scalar> tensor;
@@ -188,9 +188,9 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(
Matrix<Scalar,3,2> J_e_u3 = J_e_m3 * J_m_u;
// Error covariances induced by each of the measurement covariance // canonical units
- Matrix3s Q1 = J_e_u1 * getFeaturePrevPtr() ->getMeasurementCovariance() * J_e_u1.transpose();
- Matrix3s Q2 = J_e_u2 * getFeatureOtherPtr()->getMeasurementCovariance() * J_e_u2.transpose();
- Matrix3s Q3 = J_e_u3 * getFeaturePtr() ->getMeasurementCovariance() * J_e_u3.transpose();
+ Matrix3s Q1 = J_e_u1 * getFeaturePrev() ->getMeasurementCovariance() * J_e_u1.transpose();
+ Matrix3s Q2 = J_e_u2 * getFeatureOther()->getMeasurementCovariance() * J_e_u2.transpose();
+ Matrix3s Q3 = J_e_u3 * getFeature() ->getMeasurementCovariance() * J_e_u3.transpose();
// Total error covariance // canonical units
Matrix3s Q = Q1 + Q2 + Q3;
@@ -209,7 +209,7 @@ FactorAutodiffTrifocal::~FactorAutodiffTrifocal()
{
}
-inline FeatureBasePtr FactorAutodiffTrifocal::getFeaturePrevPtr()
+inline FeatureBasePtr FactorAutodiffTrifocal::getFeaturePrev()
{
return feature_prev_ptr_.lock();
}
diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h
index 499ffea58..6f91a034a 100644
--- a/include/base/factor/factor_base.h
+++ b/include/base/factor/factor_base.h
@@ -112,12 +112,12 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the feature constrained from
**/
- FeatureBasePtr getFeaturePtr() const;
+ FeatureBasePtr getFeature() const;
void setFeaturePtr(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/** \brief Returns a pointer to its capture
**/
- CaptureBasePtr getCapturePtr() const;
+ CaptureBasePtr getCapture() const;
/** \brief Returns the factor residual size
**/
@@ -141,22 +141,22 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the frame constrained to
**/
- FrameBasePtr getFrameOtherPtr() const { return frame_other_ptr_.lock(); }
+ FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
void setFrameOtherPtr(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
/** \brief Returns a pointer to the frame constrained to
**/
- CaptureBasePtr getCaptureOtherPtr() const { return capture_other_ptr_.lock(); }
+ CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
void setCaptureOtherPtr(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
/** \brief Returns a pointer to the feature constrained to
**/
- FeatureBasePtr getFeatureOtherPtr() const { return feature_other_ptr_.lock(); }
+ FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
void setFeatureOtherPtr(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
/** \brief Returns a pointer to the landmark constrained to
**/
- LandmarkBasePtr getLandmarkOtherPtr() const { return landmark_other_ptr_.lock(); }
+ LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
/**
@@ -214,7 +214,7 @@ inline unsigned int FactorBase::id() const
return factor_id_;
}
-inline FeatureBasePtr FactorBase::getFeaturePtr() const
+inline FeatureBasePtr FactorBase::getFeature() const
{
return feature_ptr_.lock();
}
diff --git a/include/base/factor/factor_container.h b/include/base/factor/factor_container.h
index 46d1a9330..1298cd99a 100644
--- a/include/base/factor/factor_container.h
+++ b/include/base/factor/factor_container.h
@@ -24,7 +24,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
const unsigned int _corner,
bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
FactorAutodiff<FactorContainer,3,2,1,2,1>("CONTAINER",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(),_ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
@@ -35,7 +35,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
virtual ~FactorContainer() = default;
- LandmarkContainerPtr getLandmarkPtr()
+ LandmarkContainerPtr getLandmark()
{
return lmk_ptr_.lock();
}
@@ -49,14 +49,14 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
//std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).transpose() << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
//std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
+ //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot information
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
Eigen::Matrix<T,2,2> R_landmark = Eigen::Rotation2D<T>(_landmarkO[0]).matrix();
@@ -64,7 +64,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
// Expected measurement
Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map + R_landmark * corner_position) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapturePtr()->getSensorPtr()->getOPtr()->getState()(0)) + T(lmk_ptr_.lock()->getCorner(corner_)(2));
+ T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0)) + T(lmk_ptr_.lock()->getCorner(corner_)(2));
// Error
residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
@@ -80,7 +80,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
//std::cout << "\nCONSTRAINT: " << id() << std::endl;
- //std::cout << "Feature: " << getFeaturePtr()->id() << std::endl;
+ //std::cout << "Feature: " << getFeature()->id() << std::endl;
//std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
//std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
//
@@ -100,7 +100,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
//Eigen::Matrix<T,2,1> relative_landmark_position = inverse_R_sensor * (inverse_R_robot * (landmark_position - robot_position) - sensor_position);
//for (int i=0; i < 2; i++)
// std::cout << "\n\t" << relative_landmark_position.data()[i];
- //std::cout << "\n\t" << _landmarkO[0] - _robotO[0] - T( *(getCapturePtr()->getSensorPtr()->getOPtr()->getPtr()) );
+ //std::cout << "\n\t" << _landmarkO[0] - _robotO[0] - T( *(getCapture()->getSensor()->getO()->get()) );
//std::cout << std::endl;
//
//std::cout << "expected_measurement: ";
diff --git a/include/base/factor/factor_corner_2D.h b/include/base/factor/factor_corner_2D.h
index 52558cd12..712b722c0 100644
--- a/include/base/factor/factor_corner_2D.h
+++ b/include/base/factor/factor_corner_2D.h
@@ -19,14 +19,14 @@ class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
bool _apply_loss_function = false,
FactorStatus _status = CTR_ACTIVE) :
FactorAutodiff<FactorCorner2D,3,2,1,2,1>("CORNER 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(),_ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO())
{
//
}
virtual ~FactorCorner2D() = default;
- LandmarkCorner2DPtr getLandmarkPtr()
+ LandmarkCorner2DPtr getLandmark()
{
return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
}
@@ -52,20 +52,20 @@ inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _
Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
//std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).transpose() << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
//std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
+ //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
// sensor transformation
- Eigen::Matrix<T, 2, 1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T, 2, 1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot transformation
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
// Expected measurement
Eigen::Matrix<T, 2, 1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapturePtr()->getSensorPtr()->getOPtr()->getState()(0));
+ T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0));
// Error
residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
@@ -81,7 +81,7 @@ inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _
residuals_map = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>().cast<T>() * residuals_map;
//std::cout << "\nCONSTRAINT: " << id() << std::endl;
- //std::cout << "Feature: " << getFeaturePtr()->id() << std::endl;
+ //std::cout << "Feature: " << getFeature()->id() << std::endl;
//std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
//std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
//
diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h
index 7562e6bae..89a54a3c1 100644
--- a/include/base/factor/factor_diff_drive.h
+++ b/include/base/factor/factor_diff_drive.h
@@ -44,18 +44,18 @@ public:
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
const FactorStatus _status = CTR_ACTIVE) :
- Base("DIFF DRIVE", _capture_origin_ptr->getFramePtr(), _capture_origin_ptr,
+ Base("DIFF DRIVE", _capture_origin_ptr->getFrame(), _capture_origin_ptr,
nullptr, nullptr, _processor_ptr,
_apply_loss_function, _status,
- _frame_ptr->getPPtr(), _frame_ptr->getOPtr(),
- _capture_origin_ptr->getFramePtr()->getPPtr(),
- _capture_origin_ptr->getFramePtr()->getOPtr(),
- _capture_origin_ptr->getFramePtr()->getVPtr(),
- _capture_origin_ptr->getSensorIntrinsicPtr(),
- _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr(),
- _ftr_ptr->getFramePtr()->getVPtr(),
- _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
+ _frame_ptr->getP(), _frame_ptr->getO(),
+ _capture_origin_ptr->getFrame()->getP(),
+ _capture_origin_ptr->getFrame()->getO(),
+ _capture_origin_ptr->getFrame()->getV(),
+ _capture_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getCapture()->getSensorIntrinsic()),
J_delta_calib_(_ftr_ptr->getJacobianFactor())
{
//
diff --git a/include/base/factor/factor_fix_bias.h b/include/base/factor/factor_fix_bias.h
index 9c0046ace..c34ff5bd0 100644
--- a/include/base/factor/factor_fix_bias.h
+++ b/include/base/factor/factor_fix_bias.h
@@ -21,8 +21,8 @@ class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
public:
FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getAccBiasPtr(),
- std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getGyroBiasPtr())
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
+ std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
{
// std::cout << "created FactorFixBias " << std::endl;
}
@@ -49,7 +49,7 @@ inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T
// residual
Eigen::Map<Eigen::Matrix<T,6,1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h
index cb8ca85f4..3413791a5 100644
--- a/include/base/factor/factor_odom_2D.h
+++ b/include/base/factor/factor_odom_2D.h
@@ -23,10 +23,10 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
- _frame_other_ptr->getPPtr(),
- _frame_other_ptr->getOPtr(),
- _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr())
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
diff --git a/include/base/factor/factor_odom_3D.h b/include/base/factor/factor_odom_3D.h
index 8eb2ab59b..ec14e3806 100644
--- a/include/base/factor/factor_odom_3D.h
+++ b/include/base/factor/factor_odom_3D.h
@@ -83,13 +83,13 @@ inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
_processor_ptr, // processor
_apply_loss_function,
_status,
- _ftr_current_ptr->getFramePtr()->getPPtr(), // current frame P
- _ftr_current_ptr->getFramePtr()->getOPtr(), // current frame Q
- _frame_past_ptr->getPPtr(), // past frame P
- _frame_past_ptr->getOPtr()) // past frame Q
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO(), // current frame Q
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO()) // past frame Q
{
// WOLF_TRACE("Constr ODOM 3D (f", _ftr_current_ptr->id(),
-// " F", _ftr_current_ptr->getCapturePtr()->getFramePtr()->id(),
+// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
// ") (Fo", _frame_past_ptr->id(), ")");
//
// WOLF_TRACE("delta preint: ", _ftr_current_ptr->getMeasurement().transpose());
@@ -142,15 +142,15 @@ inline bool FactorOdom3D::expectation(const T* const _p_current, const T* const
inline Eigen::VectorXs FactorOdom3D::expectation() const
{
Eigen::VectorXs exp(7);
- FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
- FrameBasePtr frm_past = getFrameOtherPtr();
- const Eigen::VectorXs frame_current_pos = frm_current->getPPtr()->getState();
- const Eigen::VectorXs frame_current_ori = frm_current->getOPtr()->getState();
- const Eigen::VectorXs frame_past_pos = frm_past->getPPtr()->getState();
- const Eigen::VectorXs frame_past_ori = frm_past->getOPtr()->getState();
-
-// std::cout << "frame_current_pos: " << frm_current->getPPtr()->getState().transpose() << std::endl;
-// std::cout << "frame_past_pos: " << frm_past->getPPtr()->getState().transpose() << std::endl;
+ FrameBasePtr frm_current = getFeature()->getFrame();
+ FrameBasePtr frm_past = getFrameOther();
+ const Eigen::VectorXs frame_current_pos = frm_current->getP()->getState();
+ const Eigen::VectorXs frame_current_ori = frm_current->getO()->getState();
+ const Eigen::VectorXs frame_past_pos = frm_past->getP()->getState();
+ const Eigen::VectorXs frame_past_ori = frm_past->getO()->getState();
+
+// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
+// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
expectation(frame_current_pos.data(),
frame_current_ori.data(),
diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h
index d81715ad7..16ed52471 100644
--- a/include/base/factor/factor_point_2D.h
+++ b/include/base/factor/factor_point_2D.h
@@ -30,7 +30,7 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
@@ -46,7 +46,7 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
* @brief getLandmarkPtr
* @return
*/
- LandmarkPolyline2DPtr getLandmarkPtr()
+ LandmarkPolyline2DPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
}
@@ -73,7 +73,7 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
* @brief getLandmarkPointPtr
* @return
*/
- StateBlockPtr getLandmarkPointPtr()
+ StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
}
@@ -120,8 +120,8 @@ inline bool FactorPoint2D::operator ()(const T* const _robotP, const T* const _r
Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginO) * landmark_position_map;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot transformation
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
index e1f15ed5c..239fdc62b 100644
--- a/include/base/factor/factor_point_to_line_2D.h
+++ b/include/base/factor/factor_point_to_line_2D.h
@@ -31,7 +31,7 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "FactorPointToLine2D" << std::endl;
@@ -42,7 +42,7 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
virtual ~FactorPointToLine2D() = default;
- LandmarkPolyline2DPtr getLandmarkPtr()
+ LandmarkPolyline2DPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
}
@@ -62,12 +62,12 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
return feature_point_id_;
}
- StateBlockPtr getLandmarkPointPtr()
+ StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
}
- StateBlockPtr getLandmarkPointAuxPtr()
+ StateBlockPtr getLandmarkPointAux()
{
return point_state_ptr_;
}
@@ -111,8 +111,8 @@ inline bool FactorPointToLine2D::operator ()(const T* const _robotP, const T* co
Eigen::Matrix<T,2,1> landmark_point_aux = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginOrientation) * landmark_aux_position_map;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot transformation
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
diff --git a/include/base/factor/factor_pose_2D.h b/include/base/factor/factor_pose_2D.h
index d15e2b9e8..93397ae4a 100644
--- a/include/base/factor/factor_pose_2D.h
+++ b/include/base/factor/factor_pose_2D.h
@@ -18,7 +18,7 @@ class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
{
public:
FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
- FactorAutodiff<FactorPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ FactorAutodiff<FactorPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
// std::cout << "created FactorPose2D " << std::endl;
}
@@ -48,7 +48,7 @@ inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/base/factor/factor_pose_3D.h b/include/base/factor/factor_pose_3D.h
index 874e4f59a..e53137aa0 100644
--- a/include/base/factor/factor_pose_3D.h
+++ b/include/base/factor/factor_pose_3D.h
@@ -17,7 +17,7 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
public:
FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
- FactorAutodiff<FactorPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ FactorAutodiff<FactorPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
@@ -48,7 +48,7 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
return true;
}
diff --git a/include/base/factor/factor_quaternion_absolute.h b/include/base/factor/factor_quaternion_absolute.h
index 616f09294..89fc55e72 100644
--- a/include/base/factor/factor_quaternion_absolute.h
+++ b/include/base/factor/factor_quaternion_absolute.h
@@ -63,7 +63,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
return true;
}
diff --git a/include/base/factor/factor_relative_2D_analytic.h b/include/base/factor/factor_relative_2D_analytic.h
index 06a9bd461..63ead17ad 100644
--- a/include/base/factor/factor_relative_2D_analytic.h
+++ b/include/base/factor/factor_relative_2D_analytic.h
@@ -23,7 +23,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = CTR_ACTIVE) :
- FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
@@ -36,7 +36,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = CTR_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _ftr_other_ptr->getFramePtr()->getPPtr(), _ftr_other_ptr->getFramePtr()->getOPtr() )
+ FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
{
//
}
@@ -49,7 +49,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = CTR_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _landmark_ptr->getPPtr(), _landmark_ptr->getOPtr())
+ FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
{
//
}
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index c4fbfcb78..ac42e2203 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -73,9 +73,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setExpectation(const Eigen::VectorXs& expectation);
// wolf tree access
- FrameBasePtr getFramePtr() const;
+ FrameBasePtr getFrame() const;
- CaptureBasePtr getCapturePtr() const;
+ CaptureBasePtr getCapture() const;
void setCapturePtr(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
@@ -115,7 +115,7 @@ inline unsigned int FeatureBase::id()
return feature_id_;
}
-inline CaptureBasePtr FeatureBase::getCapturePtr() const
+inline CaptureBasePtr FeatureBase::getCapture() const
{
return capture_ptr_.lock();
}
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index 9134d7549..90775781f 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -91,9 +91,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
void resizeStateBlockVec(unsigned int _size);
public:
- StateBlockPtr getPPtr() const;
- StateBlockPtr getOPtr() const;
- StateBlockPtr getVPtr() const;
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+ StateBlockPtr getV() const;
void setPPtr(const StateBlockPtr _p_ptr);
void setOPtr(const StateBlockPtr _o_ptr);
void setVPtr(const StateBlockPtr _v_ptr);
@@ -119,7 +119,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
public:
virtual void setProblem(ProblemPtr _problem) final;
- TrajectoryBasePtr getTrajectoryPtr() const;
+ TrajectoryBasePtr getTrajectory() const;
void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr);
FrameBasePtr getPreviousFrame() const;
@@ -193,7 +193,7 @@ inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
return state_block_vec_;
}
-inline StateBlockPtr FrameBase::getPPtr() const
+inline StateBlockPtr FrameBase::getP() const
{
return state_block_vec_[0];
}
@@ -202,7 +202,7 @@ inline void FrameBase::setPPtr(const StateBlockPtr _p_ptr)
state_block_vec_[0] = _p_ptr;
}
-inline StateBlockPtr FrameBase::getOPtr() const
+inline StateBlockPtr FrameBase::getO() const
{
return state_block_vec_[1];
}
@@ -211,7 +211,7 @@ inline void FrameBase::setOPtr(const StateBlockPtr _o_ptr)
state_block_vec_[1] = _o_ptr;
}
-inline StateBlockPtr FrameBase::getVPtr() const
+inline StateBlockPtr FrameBase::getV() const
{
return state_block_vec_[2];
}
@@ -233,7 +233,7 @@ inline void FrameBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb
state_block_vec_[_i] = _sb_ptr;
}
-inline TrajectoryBasePtr FrameBase::getTrajectoryPtr() const
+inline TrajectoryBasePtr FrameBase::getTrajectory() const
{
return trajectory_ptr_.lock();
}
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index cbaa78a83..25bd5b630 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -64,8 +64,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
std::vector<StateBlockPtr> getUsedStateBlockVec() const;
StateBlockPtr getStateBlockPtr(unsigned int _i) const;
void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr);
- StateBlockPtr getPPtr() const;
- StateBlockPtr getOPtr() const;
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
void setPPtr(const StateBlockPtr _p_ptr);
void setOPtr(const StateBlockPtr _o_ptr);
virtual void registerNewStateBlocks();
@@ -91,7 +91,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
FactorBasePtrList& getConstrainedByList();
void setMapPtr(const MapBasePtr _map_ptr);
- MapBasePtr getMapPtr();
+ MapBasePtr getMap();
};
@@ -108,7 +108,7 @@ inline void LandmarkBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
}
-inline MapBasePtr LandmarkBase::getMapPtr()
+inline MapBasePtr LandmarkBase::getMap()
{
return map_ptr_.lock();
}
@@ -161,12 +161,12 @@ inline void LandmarkBase::setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_pt
state_block_vec_[_i] = _sb_ptr;
}
-inline StateBlockPtr LandmarkBase::getPPtr() const
+inline StateBlockPtr LandmarkBase::getP() const
{
return getStateBlockPtr(0);
}
-inline StateBlockPtr LandmarkBase::getOPtr() const
+inline StateBlockPtr LandmarkBase::getO() const
{
return getStateBlockPtr(1);
}
diff --git a/include/base/problem.h b/include/base/problem.h
index 62bd70301..d96aae5e2 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -59,7 +59,7 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen getDim() const;
// Hardware branch ------------------------------------
- HardwareBasePtr getHardwarePtr();
+ HardwareBasePtr getHardware();
void addSensor(SensorBasePtr _sen_ptr);
/** \brief Factory method to install (create and add) sensors only from its properties
@@ -124,10 +124,10 @@ class Problem : public std::enable_shared_from_this<Problem>
void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
- ProcessorMotionPtr& getProcessorMotionPtr();
+ ProcessorMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectoryPtr();
+ TrajectoryBasePtr getTrajectory();
virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
const Eigen::MatrixXs& _prior_cov, //
const TimeStamp& _ts,
@@ -190,8 +190,8 @@ class Problem : public std::enable_shared_from_this<Problem>
const TimeStamp& _time_stamp);
// Frame getters
- FrameBasePtr getLastFramePtr ( );
- FrameBasePtr getLastKeyFramePtr ( );
+ FrameBasePtr getLastFrame ( );
+ FrameBasePtr getLastKeyFrame ( );
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
/** \brief Give the permission to a processor to create a new keyFrame
@@ -221,7 +221,7 @@ class Problem : public std::enable_shared_from_this<Problem>
bool priorIsSet() const;
// Map branch -----------------------------------------
- MapBasePtr getMapPtr();
+ MapBasePtr getMap();
LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
void addLandmarkList(LandmarkBasePtrList& _lmk_list);
void loadMap(const std::string& _filename_dot_yaml);
@@ -303,7 +303,7 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
+inline ProcessorMotionPtr& Problem::getProcessorMotion()
{
return processor_motion_ptr_;
}
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 05d1e9b3a..88c97e62f 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -182,8 +182,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
- SensorBasePtr getSensorPtr();
- const SensorBasePtr getSensorPtr() const;
+ SensorBasePtr getSensor();
+ const SensorBasePtr getSensor() const;
void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
virtual bool isMotion();
@@ -222,12 +222,12 @@ inline unsigned int ProcessorBase::id()
return processor_id_;
}
-inline SensorBasePtr ProcessorBase::getSensorPtr()
+inline SensorBasePtr ProcessorBase::getSensor()
{
return sensor_ptr_.lock();
}
-inline const SensorBasePtr ProcessorBase::getSensorPtr() const
+inline const SensorBasePtr ProcessorBase::getSensor() const
{
return sensor_ptr_.lock();
}
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 5cf052e30..b66feb38f 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -433,9 +433,9 @@ class ProcessorMotion : public ProcessorBase
public:
//getters
- CaptureMotionPtr getOriginPtr();
- CaptureMotionPtr getLastPtr();
- CaptureMotionPtr getIncomingPtr();
+ CaptureMotionPtr getOrigin();
+ CaptureMotionPtr getLast();
+ CaptureMotionPtr getIncoming();
Scalar getMaxTimeSpan() const;
Scalar getMaxBuffLength() const;
@@ -512,7 +512,7 @@ inline Eigen::VectorXs ProcessorMotion::getCurrentState()
inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
{
Scalar Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+ statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov()
@@ -568,17 +568,17 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
);
}
-inline CaptureMotionPtr ProcessorMotion::getOriginPtr()
+inline CaptureMotionPtr ProcessorMotion::getOrigin()
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLastPtr()
+inline CaptureMotionPtr ProcessorMotion::getLast()
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncomingPtr()
+inline CaptureMotionPtr ProcessorMotion::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index 61948f52a..2461a69fe 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -108,9 +108,9 @@ class ProcessorTracker : public ProcessorBase
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
- virtual CaptureBasePtr getOriginPtr();
- virtual CaptureBasePtr getLastPtr();
- virtual CaptureBasePtr getIncomingPtr();
+ virtual CaptureBasePtr getOrigin();
+ virtual CaptureBasePtr getLast();
+ virtual CaptureBasePtr getIncoming();
protected:
/** Pre-process incoming Capture
@@ -258,17 +258,17 @@ inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBasePtr ProcessorTracker::getOriginPtr()
+inline CaptureBasePtr ProcessorTracker::getOrigin()
{
return origin_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getLastPtr()
+inline CaptureBasePtr ProcessorTracker::getLast()
{
return last_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getIncomingPtr()
+inline CaptureBasePtr ProcessorTracker::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index 5e49f2406..dba9e9171 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -130,7 +130,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*/
virtual void establishFactors() override;
- CaptureBasePtr getPrevOriginPtr();
+ CaptureBasePtr getPrevOrigin();
bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
@@ -144,7 +144,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
const SensorBasePtr _sensor_ptr);
};
-inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOriginPtr()
+inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOrigin()
{
return prev_origin_ptr_;
}
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index c04d08467..a56b8c267 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -31,9 +31,9 @@ public:
//pointer to sensor position, orientation, bias, init vehicle position and orientation
SensorGPS(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _map_position_ptr, StateBlockPtr _map_orientation_ptr);
- StateBlockPtr getMapPPtr() const;
+ StateBlockPtr getMapP() const;
- StateBlockPtr getMapOPtr() const;
+ StateBlockPtr getMapO() const;
virtual ~SensorGPS();
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index 39e8196b9..6b8d9bc1c 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -93,7 +93,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
virtual void setProblem(ProblemPtr _problem) final;
- HardwareBasePtr getHardwarePtr();
+ HardwareBasePtr getHardware();
void setHardwarePtr(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
@@ -121,9 +121,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
StateBlockPtr getPPtr(const TimeStamp _ts);
StateBlockPtr getOPtr(const TimeStamp _ts);
StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
- StateBlockPtr getPPtr() ;
- StateBlockPtr getOPtr();
- StateBlockPtr getIntrinsicPtr();
+ StateBlockPtr getP() ;
+ StateBlockPtr getO();
+ StateBlockPtr getIntrinsic();
void setPPtr(const StateBlockPtr _p_ptr);
void setOPtr(const StateBlockPtr _o_ptr);
void setIntrinsicPtr(const StateBlockPtr _intr_ptr);
@@ -258,7 +258,7 @@ inline Eigen::MatrixXs SensorBase::getNoiseCov()
return noise_cov_;
}
-inline HardwareBasePtr SensorBase::getHardwarePtr()
+inline HardwareBasePtr SensorBase::getHardware()
{
return hardware_ptr_.lock();
}
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index c7db047c3..c5b8f4423 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -69,7 +69,7 @@ class SensorCamera : public SensorBase
inline bool SensorCamera::useRawImages()
{
- getIntrinsicPtr()->setState(pinhole_model_raw_);
+ getIntrinsic()->setState(pinhole_model_raw_);
K_ = setIntrinsicMatrix(pinhole_model_raw_);
using_raw_ = true;
@@ -78,7 +78,7 @@ inline bool SensorCamera::useRawImages()
inline bool SensorCamera::useRectifiedImages()
{
- getIntrinsicPtr()->setState(pinhole_model_rectified_);
+ getIntrinsic()->setState(pinhole_model_rectified_);
K_ = setIntrinsicMatrix(pinhole_model_rectified_);
using_raw_ = false;
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index b596cbbd7..47c45714a 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -65,7 +65,7 @@ public:
virtual void update();
- ProblemPtr getProblemPtr();
+ ProblemPtr getProblem();
protected:
diff --git a/include/base/solver_suitesparse/ccolamd_ordering.h b/include/base/solver_suitesparse/ccolamd_ordering.h
index bae2eff4d..b20b857bd 100644
--- a/include/base/solver_suitesparse/ccolamd_ordering.h
+++ b/include/base/solver_suitesparse/ccolamd_ordering.h
@@ -44,9 +44,9 @@ class CCOLAMDOrdering
IndexVector p(n + 1), A(Alen);
for (Index i = 0; i <= n; i++)
- p(i) = mat.outerIndexPtr()[i];
+ p(i) = mat.outerIndex()[i];
for (Index i = 0; i < nnz; i++)
- A(i) = mat.innerIndexPtr()[i];
+ A(i) = mat.innerIndex()[i];
// std::cout << "p = " << std::endl << p.transpose() << std::endl;
// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h
index 7952f4f89..8e576b317 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -125,13 +125,13 @@ class SolverQR
{
t_managing_ = clock();
- std::cout << "adding state unit " << _state_ptr->getPtr() << std::endl;
+ std::cout << "adding state unit " << _state_ptr->get() << std::endl;
if (!_state_ptr->isFixed())
{
nodes_.push_back(_state_ptr);
- id_2_idx_[_state_ptr->getPtr()] = nodes_.size() - 1;
+ id_2_idx_[_state_ptr->get()] = nodes_.size() - 1;
- std::cout << "idx " << id_2_idx_[_state_ptr->getPtr()] << std::endl;
+ std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl;
// Resize accumulated permutations
augmentPermutation(acc_node_permutation_, nNodes());
@@ -172,7 +172,7 @@ class SolverQR
std::vector<unsigned int> idxs;
for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++)
if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed())
- idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->getPtr()]);
+ idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]);
n_new_factors_++;
factor_locations_.push_back(A_.rows());
@@ -287,7 +287,7 @@ class SolverQR
{
if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed())
{
- unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->getPtr()];
+ unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()];
//std::cout << "estimated idx " << idx << std::endl;
//std::cout << "node_order(idx) " << node_order(idx) << std::endl;
//std::cout << "first_ordered_node " << first_ordered_node << std::endl;
@@ -377,9 +377,9 @@ class SolverQR
// finding measurements block
Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx));
//std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- //std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
+ //std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
unsigned int first_ordered_measurement =
- measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+ measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement);
unsigned int ordered_variables = A_.cols() - nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
unsigned int unordered_variables = nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
@@ -423,7 +423,7 @@ class SolverQR
// UPDATE X VALUE
for (unsigned int i = 0; i < nodes_.size(); i++)
{
- Eigen::Map < Eigen::VectorXs > x_i(nodes_.at(i)->getPtr(), nodes_.at(i)->getSize());
+ Eigen::Map < Eigen::VectorXs > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
// Zero the error
diff --git a/include/base/state_block.h b/include/base/state_block.h
index 06962ced0..09811c41e 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block.h
@@ -108,7 +108,7 @@ public:
/** \brief Returns the state local parametrization ptr
**/
- LocalParametrizationBasePtr getLocalParametrizationPtr() const;
+ LocalParametrizationBasePtr getLocalParametrization() const;
/** \brief Sets a local parametrization
**/
@@ -230,7 +230,7 @@ inline bool StateBlock::hasLocalParametrization() const
return (local_param_ptr_ != nullptr);
}
-inline LocalParametrizationBasePtr StateBlock::getLocalParametrizationPtr() const
+inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const
{
return local_param_ptr_;
}
diff --git a/include/base/track_matrix.h b/include/base/track_matrix.h
index 5aa6159c4..4fc5af1d0 100644
--- a/include/base/track_matrix.h
+++ b/include/base/track_matrix.h
@@ -59,7 +59,7 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
* The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in logarithmic time:
*
* Tracks are identified with the track ID in f->trackId()
- * Snapshots are identified with the Capture pointer in f->getCapturePtr()
+ * Snapshots are identified with the Capture pointer in f->getCapture()
*
* these fields of FeatureBase are initialized each time a feature is added to the track matrix:
*
@@ -68,7 +68,7 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
* so e.g. given a feature f,
*
* getTrack (f->trackId()) ; // returns all the track where feature f is.
- * getSnapshot(f->getCapturePtr()) ; // returns all the features in the same capture of f.
+ * getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f.
*
*/
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index ed47ccbc9..ce6dd438f 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory_base.h
@@ -37,9 +37,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
// Frames
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
FrameBasePtrList& getFrameList();
- FrameBasePtr getLastFramePtr();
- FrameBasePtr getLastKeyFramePtr();
- FrameBasePtr findLastKeyFramePtr();
+ FrameBasePtr getLastFrame();
+ FrameBasePtr getLastKeyFrame();
+ FrameBasePtr findLastKeyFrame();
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
void sortFrame(FrameBasePtr _frm_ptr);
@@ -56,12 +56,12 @@ inline FrameBasePtrList& TrajectoryBase::getFrameList()
return frame_list_;
}
-inline FrameBasePtr TrajectoryBase::getLastFramePtr()
+inline FrameBasePtr TrajectoryBase::getLastFrame()
{
return frame_list_.back();
}
-inline FrameBasePtr TrajectoryBase::getLastKeyFramePtr()
+inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
{
return last_key_frame_ptr_;
}
diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp
index f1705db6e..1321dac06 100644
--- a/src/association/association_node.cpp
+++ b/src/association/association_node.cpp
@@ -54,7 +54,7 @@ double AssociationNode::getTreeProb() const
return tree_prob_;
}
-AssociationNode* AssociationNode::upNodePtr() const
+AssociationNode* AssociationNode::upNode() const
{
//return &(*up_node_ptr_);
return up_node_ptr_;
diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp
index 1337e4bab..b425f8e5d 100644
--- a/src/association/association_tree.cpp
+++ b/src/association/association_tree.cpp
@@ -113,7 +113,7 @@ void AssociationTree::solve(std::map<unsigned int, unsigned int> & _pairs, std::
_associated_mask.at(anPtr->getDetectionIndex()) = true;
_pairs[anPtr->getDetectionIndex()] = anPtr->getTargetIndex();
}
- anPtr = anPtr->upNodePtr();
+ anPtr = anPtr->upNode();
}
}
@@ -159,7 +159,7 @@ void AssociationTree::solve(std::vector<std::pair<unsigned int, unsigned int> >
_associated_mask.at(anPtr->getDetectionIndex()) = true;
_pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
}
- anPtr = anPtr->upNodePtr();
+ anPtr = anPtr->upNode();
}
}
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index e84e13aab..cf7322fa6 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -48,7 +48,7 @@ CaptureBase::CaptureBase(const std::string& _type,
WOLF_ERROR("Provided dynamic sensor intrinsics but the sensor intrinsics are static");
}
- getSensorPtr()->setHasCapture();
+ getSensor()->setHasCapture();
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
@@ -130,25 +130,25 @@ FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _ctr_ptr)
StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const
{
- if (getSensorPtr())
+ if (getSensor())
{
if (_i < 2) // _i == 0 is position, 1 is orientation, 2 and onwards are intrinsics
- if (getSensorPtr()->extrinsicsInCaptures())
+ if (getSensor()->extrinsicsInCaptures())
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
else
- return getSensorPtr()->getStateBlockPtrStatic(_i);
+ return getSensor()->getStateBlockPtrStatic(_i);
else // 2 and onwards are intrinsics
- if (getSensorPtr()->intrinsicsInCaptures())
+ if (getSensor()->intrinsicsInCaptures())
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
else
- return getSensorPtr()->getStateBlockPtrStatic(_i);
+ return getSensor()->getStateBlockPtrStatic(_i);
}
else // No sensor associated: assume sensor params are here
{
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
index 0c3c6fe58..a2cdd817e 100644
--- a/src/capture/capture_laser_2D.cpp
+++ b/src/capture/capture_laser_2D.cpp
@@ -3,7 +3,7 @@
namespace wolf {
CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
- CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensorPtr())), scan_(_ranges)
+ CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensor())), scan_(_ranges)
{
//
}
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
index 9290da89a..7b11d9c8d 100644
--- a/src/capture/capture_wheel_joint_position.cpp
+++ b/src/capture/capture_wheel_joint_position.cpp
@@ -14,7 +14,7 @@ CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
//
const IntrinsicsDiffDrive intrinsics =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
+ *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
setDataCovariance(computeWheelJointPositionCov(getPositions(),
intrinsics.left_resolution_,
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index d81dd22ae..5f33f3be1 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -80,14 +80,14 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+ for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKey())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
all_state_blocks.push_back(sb);
// landmark state blocks
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
{
landmark_state_blocks = l_ptr->getUsedStateBlockVec();
all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
@@ -105,7 +105,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
// first create a vector containing all state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+ for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKey())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
@@ -119,7 +119,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
}
// landmark state blocks
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto sb : l_ptr->getUsedStateBlockVec())
for(auto sb2 : l_ptr->getUsedStateBlockVec())
{
@@ -135,31 +135,31 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
// state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
//
- // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
- // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
- // double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+ // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i]->get());
+ // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i+1]->get());
+ // double_pairs.emplace_back(all_state_blocks[2*i+1]->get(),all_state_blocks[2*i+1]->get());
// }
break;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
{
//robot-robot
- auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
+ auto last_key_frame = wolf_problem_->getLastKeyFrame();
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
- state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
+ state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
+ state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
+ state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO());
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
- getAssociatedMemBlockPtr(last_key_frame->getPPtr()));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
- getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
- getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
+ getAssociatedMemBlockPtr(last_key_frame->getP()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
+ getAssociatedMemBlockPtr(last_key_frame->getO()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
+ getAssociatedMemBlockPtr(last_key_frame->getO()));
// landmarks
std::vector<StateBlockPtr> landmark_state_blocks;
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
{
// load state blocks vector
landmark_state_blocks = l_ptr->getUsedStateBlockVec();
@@ -167,11 +167,11 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
{
// robot - landmark
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
- state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+ state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
+ state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
getAssociatedMemBlockPtr((*state_it)));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
getAssociatedMemBlockPtr((*state_it)));
// landmark marginal
@@ -287,7 +287,7 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
{
auto p = state_blocks_local_param_.emplace(state_ptr,
- std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrizationPtr()));
+ std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
local_parametrization_ptr = p.first->second.get();
}
@@ -406,7 +406,7 @@ void CeresManager::check()
assert(ctr_2_costfunction_[ctr_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second));
// factor - residual
- assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->getFactorPtr());
+ assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->getFactor());
// parameter blocks - state blocks
std::vector<Scalar*> param_blocks;
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index b7aca1edb..c9d2f9672 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -207,7 +207,7 @@ void QRManager::relinearizeFactor(FactorBasePtr _ctr_ptr)
// evaluate factor
std::vector<const Scalar*> ctr_states_ptr;
for (auto sb : _ctr_ptr->getStateBlockPtrVector())
- ctr_states_ptr.push_back(sb->getPtr());
+ ctr_states_ptr.push_back(sb->get());
Eigen::VectorXs residual(_ctr_ptr->getSize());
std::vector<Eigen::MatrixXs> jacobians;
_ctr_ptr->evaluate(ctr_states_ptr,residual,jacobians);
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index a9ec71153..8d5b187b6 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -84,7 +84,7 @@ void SolverManager::update()
assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
}
-ProblemPtr SolverManager::getProblemPtr()
+ProblemPtr SolverManager::getProblem()
{
return wolf_problem_;
}
diff --git a/src/examples/solver/test_iQR.cpp b/src/examples/solver/test_iQR.cpp
index 68d5ffded..b027c874d 100644
--- a/src/examples/solver/test_iQR.cpp
+++ b/src/examples/solver/test_iQR.cpp
@@ -258,8 +258,8 @@ int main(int argc, char *argv[])
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_ordered.col(min_ordered_node);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
-// std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
- int initial_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+// std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ int initial_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
SparseMatrix<double> A_ordered_partial = A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim);
solver_ordered_partial.compute(A_ordered_partial);
diff --git a/src/examples/solver/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp
index 568cc357b..2fdc1f9f2 100644
--- a/src/examples/solver/test_iQR_wolf.cpp
+++ b/src/examples/solver/test_iQR_wolf.cpp
@@ -343,8 +343,8 @@ class SolverQR
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- // std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
- int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+ // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ int first_ordered_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
int unordered_variables = nodes_.at(first_ordered_node_).location;
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index eb0de2242..93b39fe65 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -181,11 +181,11 @@ int main(int argc, char *argv[])
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
- CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblemPtr(), ceres_options);
+ CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options);
std::ofstream log_file, landmark_file; //output file
// Own solver
- SolverQR solver_(wolf_manager_QR->getProblemPtr());
+ SolverQR solver_(wolf_manager_QR->getProblem());
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
@@ -289,9 +289,9 @@ int main(int argc, char *argv[])
// draw landmarks
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ Scalar* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -324,7 +324,7 @@ int main(int argc, char *argv[])
usleep(100000 - 1e6 * dt);
// std::cout << "\nTree after step..." << std::endl;
-// wolf_manager->getProblemPtr()->print();
+// wolf_manager->getProblem()->print();
}
// DISPLAY RESULTS ============================================================================================
@@ -339,14 +339,14 @@ int main(int argc, char *argv[])
std::cout << " loop time: " << mean_times(6) << std::endl;
// std::cout << "\nTree before deleting..." << std::endl;
-// wolf_manager->getProblemPtr()->print();
+// wolf_manager->getProblem()->print();
// Draw Final result -------------------------
std::cout << "Drawing final results..." << std::endl;
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ Scalar* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -364,23 +364,23 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXs state_poses(wolf_manager_QR->getProblemPtr()->getTrajectoryPtr()->getFrameList().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblemPtr()->getTrajectoryPtr()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblemPtr()->getTrajectoryPtr()->getFrameList().end(); frame_it++)
+ Eigen::VectorXs state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
+ for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
{
if (complex_angle)
- state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr() + 1));
+ state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
else
- state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), *(*frame_it)->getOPtr()->getPtr();
+ state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
i += 3;
}
// Landmarks
std::cout << "Landmarks..." << std::endl;
i = 0;
- Eigen::VectorXs landmarks(wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().size() * 2);
- for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+ Eigen::VectorXs landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
+ for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
+ Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
landmarks.segment(i, 2) = landmark;
i += 2;
}
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index d8d91b638..10824d51e 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -263,18 +263,18 @@ int main(int argc, char** argv)
// Vehicle poses
int i = 0;
Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
- for (auto frame : *(problem.getTrajectoryPtr()->getFrameList()))
+ for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
- state_poses.segment(i, 3) << frame->getPPtr()->getVector(), frame->getOPtr()->getVector();
+ state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
i += 3;
}
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMapPtr()->getLandmarkList()->size() * 2);
- for (auto landmark : *(problem.getMapPtr()->getLandmarkList()))
+ Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
- landmarks.segment(i, 2) = landmark->getPPtr()->getVector();
+ landmarks.segment(i, 2) = landmark->getP()->getVector();
i += 2;
}
@@ -310,9 +310,9 @@ int main(int argc, char** argv)
std::getchar();
std::cout << "Problem:" << std::endl;
- std::cout << "Frames: " << problem.getTrajectoryPtr()->getFrameList().size() << std::endl;
- std::cout << "Landmarks: " << problem.getMapPtr()->getLandmarkList()->size() << std::endl;
- std::cout << "Sensors: " << problem.getHardwarePtr()->getSensorList()->size() << std::endl;
+ std::cout << "Frames: " << problem.getTrajectory()->getFrameList().size() << std::endl;
+ std::cout << "Landmarks: " << problem.getMap()->getLandmarkList()->size() << std::endl;
+ std::cout << "Sensors: " << problem.getHardware()->getSensorList()->size() << std::endl;
std::cout << " ========= END ===========" << std::endl << std::endl;
diff --git a/src/examples/test_analytic_odom_factor.cpp b/src/examples/test_analytic_odom_factor.cpp
index 373c0dd3d..344c284e2 100644
--- a/src/examples/test_analytic_odom_factor.cpp
+++ b/src/examples/test_analytic_odom_factor.cpp
@@ -126,8 +126,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_autodiff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_autodiff);
- wolf_problem_analytic->getTrajectoryPtr()->addFrame(vertex_frame_ptr_analytic);
+ wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
+ wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
// store
index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff;
index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic;
@@ -244,7 +244,7 @@ int main(int argc, char** argv)
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
FactorOdom2D* factor_ptr_autodiff = new FactorOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
- //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapturePtr()->getFramePtr()->id() << " to " << factor_ptr_autodiff->getFrameOtherPtr()->id() << std::endl;
+ //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
// add capture, feature and factor to problem
FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse());
@@ -257,7 +257,7 @@ int main(int argc, char** argv)
capture_ptr_analytic->addFeature(feature_ptr_analytic);
FactorOdom2DAnalytic* factor_ptr_analytic = new FactorOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
feature_ptr_analytic->addFactor(factor_ptr_analytic);
- //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapturePtr()->getFramePtr()->id() << " to " << factor_ptr_analytic->getFrameOtherPtr()->id() << std::endl;
+ //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
//std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
//std::cout << "covariance " << std::endl << factor_ptr_analytic->getMeasurementCovariance() << std::endl;
@@ -272,8 +272,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/src/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
index 3c48246b5..a4c75ccde 100644
--- a/src/examples/test_autodiff.cpp
+++ b/src/examples/test_autodiff.cpp
@@ -142,8 +142,8 @@ int main(int argc, char** argv)
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
- CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblemPtr(), false);
- CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblemPtr(), true);
+ CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblem(), false);
+ CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblem(), true);
std::ofstream log_file, landmark_file; //output file
//std::cout << "START TRAJECTORY..." << std::endl;
@@ -247,23 +247,23 @@ int main(int argc, char** argv)
ceres_manager_ceres->computeCovariances(ALL_MARGINALS);
ceres_manager_wolf->computeCovariances(ALL_MARGINALS);
Eigen::MatrixXs marginal_ceres(3,3), marginal_wolf(3,3);
- wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
marginal_ceres, 0, 0);
- wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_ceres, 0, 2);
- wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
- wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
+ wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_ceres, 2, 2);
- wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
marginal_wolf, 0, 0);
- wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_wolf, 0, 2);
- wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
- wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
+ wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_wolf, 2, 2);
std::cout << "CERES AUTO DIFF covariance:" << std::endl;
std::cout << marginal_ceres << std::endl;
@@ -287,9 +287,9 @@ int main(int argc, char** argv)
// // draw landmarks
// std::vector<double> landmark_vector;
-// for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
// {
-// Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+// Scalar* position_ptr = (*landmark_it)->getP()->get();
// landmark_vector.push_back(*position_ptr); //x
// landmark_vector.push_back(*(position_ptr + 1)); //y
// landmark_vector.push_back(0.2); //z
@@ -340,9 +340,9 @@ int main(int argc, char** argv)
// // Draw Final result -------------------------
// std::vector<double> landmark_vector;
-// for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
// {
-// Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+// Scalar* position_ptr = (*landmark_it)->getP()->get();
// landmark_vector.push_back(*position_ptr); //x
// landmark_vector.push_back(*(position_ptr + 1)); //y
// landmark_vector.push_back(0.2); //z
@@ -359,18 +359,18 @@ int main(int argc, char** argv)
// Vehicle poses
// int i = 0;
// Eigen::VectorXs state_poses(n_execution * 3);
-// for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameList().begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameList().end(); frame_it++)
+// for (auto frame_it = wolf_manager->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
// {
-// state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), *(*frame_it)->getOPtr()->getPtr();
+// state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
// i += 3;
// }
//
// // Landmarks
// i = 0;
-// Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().size() * 2);
-// for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+// Eigen::VectorXs landmarks(wolf_manager->getProblem()->getMap()->getLandmarkList().size() * 2);
+// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
// {
-// Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
+// Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
// landmarks.segment(i, 2) = landmark;
// i += 2;
// }
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index a98be3d47..d4afdc7d0 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -146,7 +146,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getPPtr()->getPtr();
+ Scalar* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -334,8 +334,8 @@ int main(int argc, char** argv)
//std::cout << "RENDERING..." << std::endl;
t1 = clock();
if (step % 3 == 0)
- robot.render(laser_1_processor->getLastPtr() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLastPtr()->getFeatureList(), 1, *problem.getMapPtr()->getLandmarkList(), problem.getCurrentState());
- //robot.render(laser_2_processor->getLastPtr() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLastPtr()->getFeatureListPtr(), 2, *problem.getMapPtr()->getLandmarkListPtr(), problem.getCurrentState());
+ robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
+ //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// TIME MANAGEMENT ---------------------------
@@ -361,24 +361,24 @@ int main(int argc, char** argv)
// std::cout << "\nTree before deleting..." << std::endl;
// Draw Final result -------------------------
- robot.render(laser_1_processor->getLastPtr() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLastPtr()->getFeatureList(), 1, *problem.getMapPtr()->getLandmarkList(), problem.getCurrentState());
+ robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
// Print Final result in a file -------------------------
// Vehicle poses
int i = 0;
Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
- for (auto frame : *(problem.getTrajectoryPtr()->getFrameList()))
+ for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
- state_poses.segment(i, 3) << frame->getPPtr()->getVector(), frame->getOPtr()->getVector();
+ state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
i += 3;
}
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMapPtr()->getLandmarkList()->size() * 2);
- for (auto landmark : *(problem.getMapPtr()->getLandmarkList()))
+ Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
- landmarks.segment(i, 2) = landmark->getPPtr()->getVector();
+ landmarks.segment(i, 2) = landmark->getP()->getVector();
i += 2;
}
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index a4f7cb35e..9e9ca5390 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -146,7 +146,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getPPtr()->getPtr();
+ Scalar* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -341,8 +341,8 @@ int main(int argc, char** argv)
std::cout << "RENDERING..." << std::endl;
t1 = clock();
// if (step % 3 == 0)
-// robot.render(laser_1_processor->getLastPtr() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLastPtr()->getFeatureListPtr(), 1, *problem.getMapPtr()->getLandmarkListPtr(), problem.getCurrentState());
- //robot.render(laser_2_processor->getLastPtr() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLastPtr()->getFeatureListPtr(), 2, *problem.getMapPtr()->getLandmarkListPtr(), problem.getCurrentState());
+// robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
+ //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// TIME MANAGEMENT ---------------------------
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 302204da3..306c7dfdc 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -223,7 +223,7 @@ int main(int argc, char** argv)
t.set(stamp_secs);
auto processor_diff_drive_ptr =
- std::static_pointer_cast<ProcessorDiffDrive>(wolf_problem_ptr_->getProcessorMotionPtr());
+ std::static_pointer_cast<ProcessorDiffDrive>(wolf_problem_ptr_->getProcessorMotion());
processor_diff_drive_ptr->setTimeTolerance(period_secs/2); // overwrite time tolerance based on new evidence
// Set the origin
@@ -270,19 +270,19 @@ int main(int argc, char** argv)
"-----------------------------------------");
WOLF_INFO("Integrated delta: " , /* std::fixed , std::setprecision(3),*/
- wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose());
+ wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose());
WOLF_INFO("Integrated state: " , /*std::fixed , std::setprecision(3),*/
- wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().transpose());
+ wolf_problem_ptr_->getProcessorMotion()->getCurrentState().transpose());
WOLF_INFO("Integrated std : " , /*std::fixed , std::setprecision(3),*/
- (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().
+ (wolf_problem_ptr_->getProcessorMotion()->getMotion().
delta_integr_cov_.diagonal().transpose()).array().sqrt());
// Print statistics
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- double N = (double)wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ double N = (double)wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
WOLF_INFO("t0 : " , t0.get() , " secs");
WOLF_INFO("tf : " , tf.get() , " secs");
diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp
index 75dee8a13..b3938285e 100644
--- a/src/examples/test_factor_AHP.cpp
+++ b/src/examples/test_factor_AHP.cpp
@@ -60,7 +60,7 @@ int main()
FrameBasePtr last_frame = std::make_shared<FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
std::cout << "Last frame" << std::endl;
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
// Capture
CaptureImagePtr image_ptr;
@@ -78,7 +78,7 @@ int main()
image_ptr->addFeature(feat_point_image_ptr);
FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
- //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectoryPtr()->getLastFramePtr();
+ //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectory()->getLastFrame();
// create the landmark
Eigen::Vector2s point2D;
@@ -89,12 +89,12 @@ int main()
Scalar distance = 2; // arbitrary value
Eigen::Vector4s vec_homogeneous;
- Eigen::VectorXs correction_vec = (std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getCorrectionVector();
+ Eigen::VectorXs correction_vec = (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector();
std::cout << "correction vector: " << correction_vec << std::endl;
- std::cout << "distortion vector: " << (std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getDistortionVector() << std::endl;
- point2D = pinhole::depixellizePoint(image_ptr->getSensorPtr()->getIntrinsicPtr()->getState(),point2D);
+ std::cout << "distortion vector: " << (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector() << std::endl;
+ point2D = pinhole::depixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D);
std::cout << "point2D depixellized: " << point2D.transpose() << std::endl;
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getCorrectionVector(),point2D);
+ point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector(),point2D);
std::cout << "point2D undistorted: " << point2D.transpose() << std::endl;
Eigen::Vector3s point3D;
@@ -107,7 +107,7 @@ int main()
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
std::cout << "vec_homogeneous: " << vec_homogeneous.transpose() << std::endl;
- LandmarkAHPPtr landmark = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, image_ptr->getSensorPtr(), feat_point_image_ptr->getDescriptor());
+ LandmarkAHPPtr landmark = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, image_ptr->getSensor(), feat_point_image_ptr->getDescriptor());
std::cout << "Landmark AHP created" << std::endl;
@@ -121,19 +121,19 @@ int main()
std::cout << "point2D projected: " << point2D_proj.transpose() << std::endl;
Eigen::Vector2s point2D_dist;
- point2D_dist = pinhole::distortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getDistortionVector(),point2D_proj);
+ point2D_dist = pinhole::distortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector(),point2D_proj);
std::cout << "point2D distorted: " << point2D_dist.transpose() << std::endl;
Eigen::Vector2s point2D_pix;
- point2D_pix = pinhole::pixellizePoint(image_ptr->getSensorPtr()->getIntrinsicPtr()->getState(),point2D_dist);
+ point2D_pix = pinhole::pixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D_dist);
std::cout << "point2D pixellized: " << point2D_pix.transpose() << std::endl;
Eigen::Vector2s expectation;
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getState();
- Eigen::Vector4s current_frame_o = last_frame->getOPtr()->getState();
- Eigen::Vector3s anchor_frame_p = landmark->getAnchorFrame()->getPPtr()->getState();
- Eigen::Vector4s anchor_frame_o = landmark->getAnchorFrame()->getOPtr()->getState();
- Eigen::Vector4s landmark_ = landmark->getPPtr()->getState();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getState();
+ Eigen::Vector4s current_frame_o = last_frame->getO()->getState();
+ Eigen::Vector3s anchor_frame_p = landmark->getAnchorFrame()->getP()->getState();
+ Eigen::Vector4s anchor_frame_o = landmark->getAnchorFrame()->getO()->getState();
+ Eigen::Vector4s landmark_ = landmark->getP()->getState();
factor_ptr ->expectation(current_frame_p.data(), current_frame_o.data(),
anchor_frame_p.data(), anchor_frame_o.data(),
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index c08db600a..59d98814f 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -46,15 +46,15 @@ int main(int argc, char** argv)
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t); //this also creates a keyframe at origin
- wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front()->fix(); //fix the keyframe at origin
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t); //this also creates a keyframe at origin
+ wolf_problem_ptr_->getTrajectory()->getFrameList().front()->fix(); //fix the keyframe at origin
TimeStamp ts(0);
Eigen::VectorXs origin_state = x0;
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
- imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back());
+ imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
// set variables
using namespace std;
@@ -78,14 +78,14 @@ int main(int argc, char** argv)
/// ******************************************************************************************** ///
/// factor creation
//create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
feat_imu->setCapturePtr(imu_ptr);
@@ -94,16 +94,16 @@ int main(int argc, char** argv)
feat_imu->addFactor(factor_imu);
last_frame->addConstrainedBy(factor_imu);
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
+ FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
+ Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
Eigen::Matrix<wolf::Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
@@ -114,7 +114,7 @@ int main(int argc, char** argv)
std::cout << "residuals : " << residu.transpose() << std::endl;
//reset origin of motion to new frame
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(last_frame);
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
imu_ptr->setFramePtr(last_frame);
}
/// ******************************************************************************************** ///
@@ -139,14 +139,14 @@ int main(int argc, char** argv)
/// ******************************************************************************************** ///
/// factor creation
//create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
feat_imu->setCapturePtr(imu_ptr);
@@ -155,16 +155,16 @@ int main(int argc, char** argv)
feat_imu->addFactor(factor_imu);
last_frame->addConstrainedBy(factor_imu);
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
+ FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
+ Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
Eigen::Matrix<wolf::Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
@@ -175,7 +175,7 @@ int main(int argc, char** argv)
std::cout << "residuals : " << residu.transpose() << std::endl;
//reset origin of motion to new frame
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(last_frame);
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
imu_ptr->setFramePtr(last_frame);
}
@@ -197,14 +197,14 @@ int main(int argc, char** argv)
//create the factor
//create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
feat_imu->setCapturePtr(imu_ptr);
@@ -213,16 +213,16 @@ int main(int argc, char** argv)
feat_imu->addFactor(factor_imu);
last_frame->addConstrainedBy(factor_imu);
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
+ FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
+ Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
Eigen::Matrix<wolf::Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
@@ -239,13 +239,13 @@ int main(int argc, char** argv)
///having a look at covariances
Eigen::MatrixXs predelta_cov;
predelta_cov.resize(9,9);
- predelta_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+ predelta_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
//std::cout << "predelta_cov : \n" << predelta_cov << std::endl;
///Optimization
// PRIOR
- //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front();
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front());
+ //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front();
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front());
//SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
//CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01);
//first_frame->addCapture(initial_covariance);
@@ -261,9 +261,9 @@ int main(int argc, char** argv)
// SOLVING PROBLEMS
ceres::Solver::Summary summary_diff;
std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
summary_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
std::cout << " prev_wolf_state : " << prev_wolf_state.transpose() << "\n post_wolf_state : " << post_wolf_state.transpose() << std::endl;
//std::cout << summary_wolf_diff.BriefReport() << std::endl;
std::cout << "solved!" << std::endl;
diff --git a/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
index 5e7b15d6c..621f65186 100644
--- a/src/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -135,7 +135,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getPPtr()->getPtr();
+ Scalar* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index feb794e1f..b2b61f520 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -64,7 +64,7 @@ int main(int argc, char** argv)
// std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
// std::make_shared<StateBlock>(k,false),img_width,img_height);
//
-// wolf_problem_->getHardwarePtr()->addSensor(camera_ptr);
+// wolf_problem_->getHardware()->addSensor(camera_ptr);
//
// // PROCESSOR
// ProcessorParamsImage tracker_params;
@@ -167,9 +167,9 @@ int main(int argc, char** argv)
// std::cout << summary << std::endl;
//
// std::cout << "Last key frame pose: "
-// << wolf_problem_->getLastKeyFramePtr()->getPPtr()->getState().transpose() << std::endl;
+// << wolf_problem_->getLastKeyFrame()->getP()->getState().transpose() << std::endl;
// std::cout << "Last key frame orientation: "
-// << wolf_problem_->getLastKeyFramePtr()->getOPtr()->getState().transpose() << std::endl;
+// << wolf_problem_->getLastKeyFrame()->getO()->getState().transpose() << std::endl;
//
// cv::waitKey(0);
// }
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index 87404b912..da4e5beb4 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -159,8 +159,8 @@ int main(int argc, char** argv)
mot_ptr->setData(data_odom);
sensorOdom->process(mot_ptr);
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t_middle));
- FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(t_middle));
+ FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
#else
//now impose final odometry using last timestamp of imu
data_odom << 0,-0.06,0, 0,0,0;
@@ -168,7 +168,7 @@ int main(int argc, char** argv)
mot_ptr->setData(data_odom);
sensorOdom->process(mot_ptr);
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
#endif
//#################################################### OPTIMIZATION PART
@@ -195,30 +195,30 @@ int main(int argc, char** argv)
FactorFixBiasPtr ctr_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
// ___Fix/Unfix stateblocks___
- KF1->getPPtr()->fix();
- KF1->getOPtr()->unfix();
- KF1->getVPtr()->fix();
- KF1->getAccBiasPtr()->unfix();
- KF1->getGyroBiasPtr()->unfix();
+ KF1->getP()->fix();
+ KF1->getO()->unfix();
+ KF1->getV()->fix();
+ KF1->getAccBias()->unfix();
+ KF1->getGyroBias()->unfix();
#ifdef ADD_KF3
- KF2->getPPtr()->fix();
- KF2->getOPtr()->unfix();
- KF2->getVPtr()->fix();
- KF2->getAccBiasPtr()->unfix();
- KF2->getGyroBiasPtr()->unfix();
-
- KF3->getPPtr()->unfix();
- KF3->getOPtr()->unfix();
- KF3->getVPtr()->fix();
- KF3->getAccBiasPtr()->unfix();
- KF3->getGyroBiasPtr()->unfix();
+ KF2->getP()->fix();
+ KF2->getO()->unfix();
+ KF2->getV()->fix();
+ KF2->getAccBias()->unfix();
+ KF2->getGyroBias()->unfix();
+
+ KF3->getP()->unfix();
+ KF3->getO()->unfix();
+ KF3->getV()->fix();
+ KF3->getAccBias()->unfix();
+ KF3->getGyroBias()->unfix();
#else
- KF2->getPPtr()->unfix();
- KF2->getOPtr()->unfix();
- KF2->getVPtr()->fix();
- KF2->getAccBiasPtr()->unfix();
- KF2->getGyroBiasPtr()->unfix();
+ KF2->getP()->unfix();
+ KF2->getO()->unfix();
+ KF2->getV()->fix();
+ KF2->getAccBias()->unfix();
+ KF2->getGyroBias()->unfix();
#endif
#ifdef OUTPUT_RESULTS
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 8114ae02b..2cdffa833 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -183,7 +183,7 @@ int main(int argc, char** argv)
odom_data_input.close();
//A KeyFrame should have been created (depending on time_span in processors). get all frames
- FrameBasePtrList trajectory = problem->getTrajectoryPtr()->getFrameList();
+ FrameBasePtrList trajectory = problem->getTrajectory()->getFrameList();
//#################################################### OPTIMIZATION PART
@@ -218,17 +218,17 @@ int main(int argc, char** argv)
frame_imu = std::static_pointer_cast<FrameIMU>(frame);
if(frame_imu->isKey())
{
- frame_imu->getPPtr()->fix();
- frame_imu->getOPtr()->unfix();
- frame_imu->getVPtr()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
- frame_imu->getVPtr()->fix();
- frame_imu->getAccBiasPtr()->unfix();
- frame_imu->getGyroBiasPtr()->unfix();
+ frame_imu->getP()->fix();
+ frame_imu->getO()->unfix();
+ frame_imu->getV()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
+ frame_imu->getV()->fix();
+ frame_imu->getAccBias()->unfix();
+ frame_imu->getGyroBias()->unfix();
}
}
//KF1 (origin) needs to be also fixed in position
- KF1->getPPtr()->fix();
+ KF1->getP()->fix();
#ifdef OUTPUT_RESULTS
// ___OUTPUT___
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index 968a304c5..e7ed6a915 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -112,8 +112,8 @@ int main(int argc, char** argv)
processor_ptr_odom3D->setOrigin(origin_KF);
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
//===================================================== PROCESS DATA
// PROCESS DATA
@@ -147,9 +147,9 @@ int main(int argc, char** argv)
}
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
//Finally, process the only one odom input
mot_ptr->setTimeStamp(ts);
@@ -157,7 +157,7 @@ int main(int argc, char** argv)
sen_odom3D->process(mot_ptr);
clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
//closing file
imu_data_input.close();
@@ -170,20 +170,20 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x_origin.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+ << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
/*TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();*/
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();*/
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
@@ -194,9 +194,9 @@ int main(int argc, char** argv)
std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
std::cout << "\t\t\t ______solving______" << std::endl;
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -212,7 +212,7 @@ int main(int argc, char** argv)
Eigen::MatrixXs covX(16,16);
Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
- wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList();
+ wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
for(FrameBasePtr frm_ptr : frame_list)
{
if(frm_ptr->isKey())
@@ -224,11 +224,11 @@ int main(int argc, char** argv)
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getVPtr(), frmIMU_ptr->getVPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBiasPtr(), frmIMU_ptr->getAccBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBiasPtr(), frmIMU_ptr->getGyroBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
covX.block(13,13,3,3) = cov3;
for(int i = 0; i<16; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index 41b854f9e..7683b1820 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -137,8 +137,8 @@ int main(int argc, char** argv)
expected_final_state[4] >> expected_final_state[5] >> expected_final_state[7] >> expected_final_state[8] >> expected_final_state[9];
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
//===================================================== PROCESS DATA
// PROCESS DATA
@@ -206,7 +206,7 @@ int main(int argc, char** argv)
}
clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
//closing file
imu_data_input.close();
@@ -225,20 +225,20 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x_origin.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
+ << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
@@ -249,17 +249,17 @@ int main(int argc, char** argv)
std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
std::cout << "\t\t\t ______solving______" << std::endl;
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport
std::cout << report << std::endl;
- last_KF->getAccBiasPtr()->fix();
- last_KF->getGyroBiasPtr()->fix();
+ last_KF->getAccBias()->fix();
+ last_KF->getGyroBias()->fix();
std::cout << "\t\t\t solving after fixBias" << std::endl;
report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport
@@ -275,7 +275,7 @@ int main(int argc, char** argv)
Eigen::MatrixXs covX(16,16);
Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
- wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList();
+ wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
for(FrameBasePtr frm_ptr : frame_list)
{
if(frm_ptr->isKey())
@@ -287,11 +287,11 @@ int main(int argc, char** argv)
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getVPtr(), frmIMU_ptr->getVPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBiasPtr(), frmIMU_ptr->getAccBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBiasPtr(), frmIMU_ptr->getGyroBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
covX.block(13,13,3,3) = cov3;
for(int i = 0; i<16; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -331,19 +331,19 @@ int main(int argc, char** argv)
{
if(ctr_ptr->getTypeId() == CTR_IMU)
{
- //Eigen::VectorXs prev_KF_state(ctr_ptr->getFrameOtherPtr()->getState());
- //Eigen::VectorXs curr_KF_state(ctr_ptr->getFeaturePtr()->getFramePtr()->getState());
- p1 = ctr_ptr->getFrameOtherPtr()->getPPtr()->getState();
- q1_vec = ctr_ptr->getFrameOtherPtr()->getOPtr()->getState();
- v1 = ctr_ptr->getFrameOtherPtr()->getVPtr()->getState();
- ab1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOtherPtr())->getAccBiasPtr()->getState();
- wb1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOtherPtr())->getGyroBiasPtr()->getState();
-
- p2 = ctr_ptr->getFeaturePtr()->getFramePtr()->getPPtr()->getState();
- q2_vec = ctr_ptr->getFeaturePtr()->getFramePtr()->getOPtr()->getState();
- v2 = ctr_ptr->getFeaturePtr()->getFramePtr()->getVPtr()->getState();
- ab2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeaturePtr()->getFramePtr())->getAccBiasPtr()->getState();
- wb2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeaturePtr()->getFramePtr())->getGyroBiasPtr()->getState();
+ //Eigen::VectorXs prev_KF_state(ctr_ptr->getFrameOther()->getState());
+ //Eigen::VectorXs curr_KF_state(ctr_ptr->getFeature()->getFrame()->getState());
+ p1 = ctr_ptr->getFrameOther()->getP()->getState();
+ q1_vec = ctr_ptr->getFrameOther()->getO()->getState();
+ v1 = ctr_ptr->getFrameOther()->getV()->getState();
+ ab1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOther())->getAccBias()->getState();
+ wb1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOther())->getGyroBias()->getState();
+
+ p2 = ctr_ptr->getFeature()->getFrame()->getP()->getState();
+ q2_vec = ctr_ptr->getFeature()->getFrame()->getO()->getState();
+ v2 = ctr_ptr->getFeature()->getFrame()->getV()->getState();
+ ab2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeature()->getFrame())->getAccBias()->getState();
+ wb2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeature()->getFrame())->getGyroBias()->getState();
std::static_pointer_cast<FactorIMU>(ctr_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
std::cout << "IMU residuals : " << IMU_residuals.transpose() << std::endl;
diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index 8f049c8d6..05bd16597 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -35,8 +35,8 @@ int main()
sen_ftr->addProcessor(prc_ftr);
prc_ftr->setTimeTolerance(0.1);
- cout << "Motion sensor : " << problem->getProcessorMotionPtr()->getSensorPtr()->getName() << endl;
- cout << "Motion processor : " << problem->getProcessorMotionPtr()->getName() << endl;
+ cout << "Motion sensor : " << problem->getProcessorMotion()->getSensor()->getName() << endl;
+ cout << "Motion processor : " << problem->getProcessorMotion()->getName() << endl;
TimeStamp t(0);
cout << "=======================\n>> TIME: " << t.get() << endl;
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index 7990919ca..e9a2af536 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -25,8 +25,8 @@ void print(MapBase& _map)
if (lmk_ptr->getType() == "POLYLINE 2D")
{
LandmarkPolyline2DPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
- std::cout << "\npos: " << poly_ptr->getPPtr()->getState().transpose() << " -- fixed: " << poly_ptr->getPPtr()->isFixed();
- std::cout << "\nori: " << poly_ptr->getOPtr()->getState().transpose() << " -- fixed: " << poly_ptr->getOPtr()->isFixed();
+ std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
+ std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
std::cout << "\nn points: " << poly_ptr->getNPoints();
std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
@@ -38,7 +38,7 @@ void print(MapBase& _map)
else if (lmk_ptr->getType() == "AHP")
{
LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
- std::cout << "\npos: " << ahp_ptr->getPPtr()->getState().transpose() << " -- fixed: " << ahp_ptr->getPPtr()->isFixed();
+ std::cout << "\npos: " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
std::cout << "\ndescript: " << ahp_ptr->getCvDescriptor().t();
break;
}
@@ -58,7 +58,7 @@ int main()
v << 1, 2, 3, 4;
cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
LandmarkAHP lmk_1(v, nullptr, nullptr, d);
- std::cout << "Pos 1 = " << lmk_1.getPPtr()->getState().transpose() << std::endl;
+ std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
YAML::Node n = lmk_1.saveToYaml();
@@ -66,7 +66,7 @@ int main()
std::cout << "Des n = " << n["descriptor"].as<VectorXs>().transpose() << std::endl;
LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
- std::cout << "Pos 2 = " << lmk_2.getPPtr()->getState().transpose() << std::endl;
+ std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
std::string filename;
@@ -81,21 +81,21 @@ int main()
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
- print(*(problem->getMapPtr()));
+ print(*(problem->getMap()));
filename = wolf_config + "/map_polyline_example_write.yaml";
std::cout << "Writing map to file: " << filename << std::endl;
std::string thisfile = __FILE__;
- problem->getMapPtr()->save(filename, "Example generated by test file " + thisfile);
+ problem->getMap()->save(filename, "Example generated by test file " + thisfile);
std::cout << "Clearing map... " << std::endl;
- problem->getMapPtr()->getLandmarkList().clear();
+ problem->getMap()->getLandmarkList().clear();
std::cout << "Re-reading map from file: " << filename << std::endl;
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
- print(*(problem->getMapPtr()));
+ print(*(problem->getMap()));
return 0;
}
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index 08db29a81..0e397cac3 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -105,7 +105,7 @@ int main(int argc, char** argv)
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
Eigen::VectorXs IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, IntrinsicsBasePtr());
+ SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, IntrinsicsBase());
wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Time and data variables
@@ -118,7 +118,7 @@ int main(int argc, char** argv)
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0, 1,0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) );
@@ -165,10 +165,10 @@ int main(int argc, char** argv)
Eigen::VectorXs x_debug;
TimeStamp ts;
- delta_debug = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_;
- delta_integr_debug = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
- x_debug = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ delta_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_;
+ delta_integr_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
+ x_debug = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
if(debug_results)
debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
@@ -188,11 +188,11 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x0.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+ << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
@@ -204,9 +204,9 @@ int main(int argc, char** argv)
#endif
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index f9b57a323..100614458 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -73,7 +73,7 @@ int main(int argc, char** argv)
ProblemPtr problem_ptr_ = Problem::create("PQVBB 3D");
Eigen::VectorXs extrinsics(7);
extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBasePtr());
+ SensorBasePtr sensor_ptr = problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBase());
problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Time and data variables
@@ -93,7 +93,7 @@ int main(int argc, char** argv)
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases
- problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+ problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
@@ -132,18 +132,18 @@ int main(int argc, char** argv)
<< data.transpose() << std::endl;
std::cout << "Current delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
- << problem_ptr_->getProcessorMotionPtr()->getMotion().delta_.transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getMotion().delta_.transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- Eigen::VectorXs x = problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ Eigen::VectorXs x = problem_ptr_->getProcessorMotion()->getCurrentState();
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x.head(10).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+ << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
std::cout << std::endl;
@@ -155,10 +155,10 @@ int main(int argc, char** argv)
Eigen::VectorXs x_debug;
TimeStamp ts;
- delta_debug = problem_ptr_->getProcessorMotionPtr()->getMotion().delta_;
- delta_integr_debug = problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
- x_debug = problem_ptr_->getProcessorMotionPtr()->getCurrentState();
- ts = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ delta_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_;
+ delta_integr_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
+ x_debug = problem_ptr_->getProcessorMotion()->getCurrentState();
+ ts = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
if(debug_results)
debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
@@ -178,11 +178,11 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x0.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
// std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
-// << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+// << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
@@ -194,9 +194,9 @@ int main(int argc, char** argv)
#endif
TimeStamp t0, tf;
- t0 = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = problem_ptr_->getProcessorMotion()->getBuffer().get().size();
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
index e876a21d9..6cf92cef2 100644
--- a/src/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -49,7 +49,7 @@ int main(int argc, char** argv)
Eigen::VectorXs x0(16);
x0 << 0,1,0, 0,0,0,1, 1,0,0, 0,0,.000, 0,0,.000; // P Q V B B
- //wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+ //wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
//CaptureIMU* imu_ptr;
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index c9321b51e..ffeb5ce92 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -85,7 +85,7 @@ int main (int argc, char** argv)
}
problem->print(1,0,1,0);
-// for (auto frm : problem->getTrajectoryPtr()->getFrameList())
+// for (auto frm : problem->getTrajectory()->getFrameList())
// {
// frm->setState(problem->zeroState());
// }
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index b09aad968..81900c7ef 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -20,8 +20,8 @@ void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
{
using namespace wolf;
std::cout << "\n-----------\nWolf tree begin" << std::endl;
- std::cout << "Hrd: " << wolf_problem_ptr_->getHardwarePtr()->getProblem() << std::endl;
- for (SensorBasePtr sen : wolf_problem_ptr_->getHardwarePtr()->getSensorList())
+ std::cout << "Hrd: " << wolf_problem_ptr_->getHardware()->getProblem() << std::endl;
+ for (SensorBasePtr sen : wolf_problem_ptr_->getHardware()->getSensorList())
{
std::cout << "\tSen: " << sen->getProblem() << std::endl;
for (ProcessorBasePtr prc : sen->getProcessorList())
@@ -29,8 +29,8 @@ void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
std::cout << "\t\tPrc: " << prc->getProblem() << std::endl;
}
}
- std::cout << "Trj: " << wolf_problem_ptr_->getTrajectoryPtr()->getProblem() << std::endl;
- for (FrameBasePtr frm : wolf_problem_ptr_->getTrajectoryPtr()->getFrameList())
+ std::cout << "Trj: " << wolf_problem_ptr_->getTrajectory()->getProblem() << std::endl;
+ for (FrameBasePtr frm : wolf_problem_ptr_->getTrajectory()->getFrameList())
{
std::cout << "\tFrm: " << frm->getProblem() << std::endl;
for (CaptureBasePtr cap : frm->getCaptureList())
@@ -46,8 +46,8 @@ void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
}
}
}
- std::cout << "Map: " << wolf_problem_ptr_->getMapPtr()->getProblem() << std::endl;
- for (LandmarkBasePtr lmk : wolf_problem_ptr_->getMapPtr()->getLandmarkList())
+ std::cout << "Map: " << wolf_problem_ptr_->getMap()->getProblem() << std::endl;
+ for (LandmarkBasePtr lmk : wolf_problem_ptr_->getMap()->getLandmarkList())
{
std::cout << "\tLmk: " << lmk->getProblem() << std::endl;
}
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index fab346be6..d8b22c18d 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -31,7 +31,7 @@ void cleanupMap(const ProblemPtr& _problem, const TimeStamp& _t, Scalar _dt_max,
SizeEigen _min_factors)
{
std::list<LandmarkBasePtr> lmks_to_remove;
- for (auto lmk : _problem->getMapPtr()->getLandmarkList())
+ for (auto lmk : _problem->getMap()->getLandmarkList())
{
TimeStamp t0 = std::static_pointer_cast<LandmarkAHP>(lmk)->getAnchorFrame()->getTimeStamp();
if (_t - t0 > _dt_max)
@@ -99,7 +99,7 @@ int main(int argc, char** argv)
// Origin Key Frame is fixed
TimeStamp t = 0;
FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
- problem->getProcessorMotionPtr()->setOrigin(origin_frame);
+ problem->getProcessorMotion()->setOrigin(origin_frame);
origin_frame->fix();
std::cout << "t: " << 0 << " \t\t\t x = ( " << problem->getCurrentState().transpose() << ")" << std::endl;
@@ -161,7 +161,7 @@ int main(int argc, char** argv)
cap_odo->setTimeStamp(t);
// previous state
- FrameBasePtr prev_key_fr_ptr = problem->getLastKeyFramePtr();
+ FrameBasePtr prev_key_fr_ptr = problem->getLastKeyFrame();
// Eigen::Vector7s x_prev = problem->getCurrentState();
Eigen::Vector7s x_prev = prev_key_fr_ptr->getState();
@@ -169,11 +169,11 @@ int main(int argc, char** argv)
FrameBasePtr prev_prev_key_fr_ptr = nullptr;
Vector7s x_prev_prev;
Vector7s dx;
- for (auto f_it = problem->getTrajectoryPtr()->getFrameList().rbegin(); f_it != problem->getTrajectoryPtr()->getFrameList().rend(); f_it++)
+ for (auto f_it = problem->getTrajectory()->getFrameList().rbegin(); f_it != problem->getTrajectory()->getFrameList().rend(); f_it++)
if ((*f_it) == prev_key_fr_ptr)
{
f_it++;
- if (f_it != problem->getTrajectoryPtr()->getFrameList().rend())
+ if (f_it != problem->getTrajectory()->getFrameList().rend())
{
prev_prev_key_fr_ptr = (*f_it);
}
@@ -227,9 +227,9 @@ int main(int argc, char** argv)
// Solve -----------------------------------------------
// solve only when new KFs are added
- if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
+ if (problem->getTrajectory()->getFrameList().size() > number_of_KFs)
{
- number_of_KFs = problem->getTrajectoryPtr()->getFrameList().size();
+ number_of_KFs = problem->getTrajectory()->getFrameList().size();
std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << summary << std::endl;
}
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index 967d37006..e046765f0 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -23,7 +23,7 @@ using namespace wolf;
void printFrames(ProblemPtr _problem_ptr)
{
std::cout << "TRAJECTORY:" << std::endl;
- for (auto frm : _problem_ptr->getTrajectoryPtr()->getFrameList())
+ for (auto frm : _problem_ptr->getTrajectory()->getFrameList())
std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl;
}
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index ca520dd1f..fd7093130 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -187,7 +187,7 @@ int main(int argc, char** argv)
// Wolf problem
FrameBasePtr last_frame_ptr, frame_from_ptr, frame_to_ptr;
ProblemPtr bl_problem_ptr = Problem::create("PO_2D");
- SensorBasePtr sensor_ptr = bl_problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBasePtr());
+ SensorBasePtr sensor_ptr = bl_problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBase());
// Ceres wrapper
std::string bl_summary, sp_summary;
@@ -252,7 +252,7 @@ int main(int argc, char** argv)
if (edge_from == last_frame_ptr->id())
frame_from_ptr = last_frame_ptr;
else
- for (auto frm_rit = bl_problem_ptr->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->getFrameList().rend(); frm_rit++)
+ for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
if ((*frm_rit)->id() == edge_from)
{
frame_from_ptr = *frm_rit;
@@ -261,7 +261,7 @@ int main(int argc, char** argv)
if (edge_to == last_frame_ptr->id())
frame_to_ptr = last_frame_ptr;
else
- for (auto frm_rit = bl_problem_ptr->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->getFrameList().rend(); frm_rit++)
+ for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
if ((*frm_rit)->id() == edge_to)
{
frame_to_ptr = *frm_rit;
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
index 4d2aa60cc..5885553ed 100644
--- a/src/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -24,9 +24,9 @@ int main (void)
FrameBase pqv(t,pp,op,vp);
- std::cout << "P local param: " << pqv.getPPtr()->getLocalParametrizationPtr() << std::endl;
- std::cout << "Q local param: " << pqv.getOPtr()->getLocalParametrizationPtr() << std::endl;
- std::cout << "V local param: " << pqv.getVPtr()->getLocalParametrizationPtr() << std::endl;
+ std::cout << "P local param: " << pqv.getP()->getLocalParametrization() << std::endl;
+ std::cout << "Q local param: " << pqv.getO()->getLocalParametrization() << std::endl;
+ std::cout << "V local param: " << pqv.getV()->getLocalParametrization() << std::endl;
// delete pp;
// delete op;
diff --git a/src/examples/test_virtual_hierarchy.cpp b/src/examples/test_virtual_hierarchy.cpp
index d83183b84..fb1b248ce 100644
--- a/src/examples/test_virtual_hierarchy.cpp
+++ b/src/examples/test_virtual_hierarchy.cpp
@@ -43,7 +43,7 @@ class ChildOf : virtual public N
protected:
ChildOf(Parent* _up_ptr) : up_ptr_(_up_ptr) { }
virtual ~ChildOf() { }
- Parent* upPtr() { return up_ptr_; }
+ Parent* up() { return up_ptr_; }
};
/**
diff --git a/src/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
index 845475ade..1fa76e585 100644
--- a/src/examples/test_wolf_autodiffwrapper.cpp
+++ b/src/examples/test_wolf_autodiffwrapper.cpp
@@ -114,8 +114,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_ceres_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_wolf_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_ceres_diff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_ceres_diff);
- wolf_problem_wolf_diff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_wolf_diff);
+ wolf_problem_ceres_diff->getTrajectory()->addFrame(vertex_frame_ptr_ceres_diff);
+ wolf_problem_wolf_diff->getTrajectory()->addFrame(vertex_frame_ptr_wolf_diff);
// store
index_2_frame_ptr_ceres_diff[vertex_index] = vertex_frame_ptr_ceres_diff;
index_2_frame_ptr_wolf_diff[vertex_index] = vertex_frame_ptr_wolf_diff;
@@ -243,7 +243,7 @@ int main(int argc, char** argv)
FactorOdom2D* factor_ptr_wolf_diff = new FactorOdom2D(feature_ptr_wolf_diff, frame_old_ptr_wolf_diff);
feature_ptr_ceres_diff->addFactor(factor_ptr_ceres_diff);
feature_ptr_wolf_diff->addFactor(factor_ptr_wolf_diff);
- //std::cout << "Added edge! " << factor_ptr_wolf_diff->id() << " from vertex " << factor_ptr_wolf_diff->getCapturePtr()->getFramePtr()->id() << " to " << factor_ptr_wolf_diff->getFrameToPtr()->id() << std::endl;
+ //std::cout << "Added edge! " << factor_ptr_wolf_diff->id() << " from vertex " << factor_ptr_wolf_diff->getCapture()->getFrame()->id() << " to " << factor_ptr_wolf_diff->getFrameTo()->id() << std::endl;
//std::cout << "vector " << factor_ptr_wolf_diff->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
//std::cout << "covariance " << std::endl << factor_ptr_wolf_diff->getMeasurementCovariance() << std::endl;
@@ -258,8 +258,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_ceres_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_ceres_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_wolf_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_wolf_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_ceres_diff->addCapture(initial_covariance_ceres_diff);
@@ -280,13 +280,13 @@ int main(int argc, char** argv)
// SOLVING PROBLEMS
std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_ceres_state = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs prev_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
summary_ceres_diff = ceres_manager_ceres_diff->solve();
- Eigen::VectorXs post_ceres_state = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs post_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
//std::cout << summary_ceres_diff.BriefReport() << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs prev_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
summary_wolf_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs post_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
//std::cout << summary_wolf_diff.BriefReport() << std::endl;
std::cout << "solved!" << std::endl;
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 3ada83334..b35d0dc1b 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -78,7 +78,7 @@ int main(void)
problem->installSensor("CAMERA", "rear camera", pq_3d, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
// print available sensors
- for (auto sen : problem->getHardwarePtr()->getSensorList())
+ for (auto sen : problem->getHardware()->getSensorList())
{
cout << "Sensor " << setw(2) << left << sen->id()
<< " | type " << setw(8) << sen->getType()
@@ -95,12 +95,12 @@ int main(void)
// problem->createProcessor("GPS", "GPS pseudoranges", "GPS raw");
// print installed processors
- for (auto sen : problem->getHardwarePtr()->getSensorList())
+ for (auto sen : problem->getHardware()->getSensorList())
for (auto prc : sen->getProcessorList())
cout << "Processor " << setw(2) << left << prc->id()
<< " | type : " << setw(8) << prc->getType()
<< " | name: " << setw(17) << prc->getName()
- << " | bound to sensor " << setw(2) << prc->getSensorPtr()->id() << ": " << prc->getSensorPtr()->getName() << endl;
+ << " | bound to sensor " << setw(2) << prc->getSensor()->id() << ": " << prc->getSensor()->getName() << endl;
return 0;
}
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index eb1fb30d3..2bfe0055a 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -132,8 +132,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_full->getTrajectoryPtr()->addFrame(vertex_frame_ptr_full);
- wolf_problem_prun->getTrajectoryPtr()->addFrame(vertex_frame_ptr_prun);
+ wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
+ wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
@@ -263,18 +263,18 @@ int main(int argc, char** argv)
FactorOdom2D* factor_ptr_prun = new FactorOdom2D(feature_ptr_prun, frame_old_ptr_prun);
feature_ptr_full->addFactor(factor_ptr_full);
feature_ptr_prun->addFactor(factor_ptr_prun);
- //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapturePtr()->getFramePtr()->id() << " to " << factor_ptr_prun->getFrameToPtr()->id() << std::endl;
+ //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl;
//std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
//std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
- Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
- Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
- Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
+ Scalar xi = *(frame_old_ptr_prun->getP()->get());
+ Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
+ Scalar thi = *(frame_old_ptr_prun->getO()->get());
Scalar si = sin(thi);
Scalar ci = cos(thi);
- Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
- Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
+ Scalar xj = *(frame_new_ptr_prun->getP()->get());
+ Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
Eigen::MatrixXs Ji(3,3), Jj(3,3);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -301,8 +301,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
@@ -316,7 +316,7 @@ int main(int argc, char** argv)
// COMPUTE COVARIANCES
FactorBasePtrList factors;
- wolf_problem_prun->getTrajectoryPtr()->getFactorList(factors);
+ wolf_problem_prun->getTrajectory()->getFactorList(factors);
// Manual covariance computation
t1 = clock();
Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A
@@ -338,35 +338,35 @@ int main(int argc, char** argv)
if ((*c_it)->getCategory() != CTR_FRAME) continue;
// ii (old)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3) << std::endl;
+// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
// jj (new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
// ij (old-new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
//jacobian
- xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr();
- yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1);
- thi = *(*c_it)->getFrameOtherPtr()->getOPtr()->getPtr();
+ xi = *(*c_it)->getFrameOther()->getP()->get();
+ yi = *((*c_it)->getFrameOther()->getP()->get()+1);
+ thi = *(*c_it)->getFrameOther()->getO()->get();
si = sin(thi);
ci = cos(thi);
- xj = *(*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr();
- yj = *((*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr()+1);
+ xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
+ yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -378,7 +378,7 @@ int main(int argc, char** argv)
//std::cout << "Jj" << std::endl << Jj << std::endl;
// Measurement covariance
- Sigma_z = (*c_it)->getFeaturePtr()->getMeasurementCovariance();
+ Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
//std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
//std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index caf906c4d..c7b7efa86 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -150,8 +150,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_full->getTrajectoryPtr()->addFrame(vertex_frame_ptr_full);
- wolf_problem_prun->getTrajectoryPtr()->addFrame(vertex_frame_ptr_prun);
+ wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
+ wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
@@ -282,19 +282,19 @@ int main(int argc, char** argv)
FactorOdom2DAnalytic* factor_ptr_prun = new FactorOdom2DAnalytic(feature_ptr_prun, frame_old_ptr_prun);
feature_ptr_full->addFactor(factor_ptr_full);
feature_ptr_prun->addFactor(factor_ptr_prun);
- //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapturePtr()->getFramePtr()->id() << " to " << factor_ptr_prun->getFrameOtherPtr()->id() << std::endl;
+ //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameOther()->id() << std::endl;
//std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
//std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
t1 = clock();
- Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
- Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
- Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
+ Scalar xi = *(frame_old_ptr_prun->getP()->get());
+ Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
+ Scalar thi = *(frame_old_ptr_prun->getO()->get());
Scalar si = sin(thi);
Scalar ci = cos(thi);
- Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
- Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
+ Scalar xj = *(frame_new_ptr_prun->getP()->get());
+ Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
Eigen::MatrixXs Ji(3,3), Jj(3,3);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -322,8 +322,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
@@ -339,7 +339,7 @@ int main(int argc, char** argv)
// COMPUTE COVARIANCES
FactorBasePtrList factors;
- wolf_problem_prun->getTrajectoryPtr()->getFactorList(factors);
+ wolf_problem_prun->getTrajectory()->getFactorList(factors);
// Manual covariance computation
t1 = clock();
Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A
@@ -361,35 +361,35 @@ int main(int argc, char** argv)
if ((*c_it)->getCategory() != CTR_FRAME) continue;
// Measurement covariance
- Sigma_z = (*c_it)->getFeaturePtr()->getMeasurementCovariance();
+ Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
//std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
//std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
// NEW WAY
// State covariance
//11
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_11);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_11);
//12
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_12);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_12);
//13
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_13);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_13);
//14
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_14);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_14);
//22
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_22);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_22);
//23
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_23);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_23);
//24
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_24);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_24);
//33
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_33);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_33);
//34
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_34);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_34);
//44
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_44);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_44);
// std::cout << "Sigma_11" << std::endl << Sigma_11 << std::endl;
// std::cout << "Sigma_12" << std::endl << Sigma_12 << std::endl;
@@ -452,35 +452,35 @@ int main(int argc, char** argv)
// OLD WAY
// ii (old)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3) << std::endl;
+// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
// jj (new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
// ij (old-new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
//jacobian
- xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr();
- yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1);
- thi = *(*c_it)->getFrameOtherPtr()->getOPtr()->getPtr();
+ xi = *(*c_it)->getFrameOther()->getP()->get();
+ yi = *((*c_it)->getFrameOther()->getP()->get()+1);
+ thi = *(*c_it)->getFrameOther()->getO()->get();
si = sin(thi);
ci = cos(thi);
- xj = *(*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr();
- yj = *((*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr()+1);
+ xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
+ yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -521,12 +521,12 @@ int main(int argc, char** argv)
}
// PRUNNING
- std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectoryPtr()->getFrameList().size(), false);
+ std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectory()->getFrameList().size(), false);
for (auto c_it = ordered_ctr_ptr.begin(); c_it != ordered_ctr_ptr.end(); c_it++ )
{
// Check heuristic: factor do not share node with any inactive factor
- unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()];
- unsigned int& index_frame_other = frame_ptr_2_index_prun[(*c_it)->getFrameOtherPtr()];
+ unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()];
+ unsigned int& index_frame_other = frame_ptr_2_index_prun[(*c_it)->getFrameOther()];
if (!any_inactive_in_frame[index_frame] && !any_inactive_in_frame[index_frame_other])
{
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 71e45b360..ba158d9af 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -99,22 +99,22 @@ void FactorBase::remove()
const Eigen::VectorXs& FactorBase::getMeasurement() const
{
- return getFeaturePtr()->getMeasurement();
+ return getFeature()->getMeasurement();
}
const Eigen::MatrixXs& FactorBase::getMeasurementCovariance() const
{
- return getFeaturePtr()->getMeasurementCovariance();
+ return getFeature()->getMeasurementCovariance();
}
const Eigen::MatrixXs& FactorBase::getMeasurementSquareRootInformationUpper() const
{
- return getFeaturePtr()->getMeasurementSquareRootInformationUpper();
+ return getFeature()->getMeasurementSquareRootInformationUpper();
}
-CaptureBasePtr FactorBase::getCapturePtr() const
+CaptureBasePtr FactorBase::getCapture() const
{
- return getFeaturePtr()->getCapturePtr();
+ return getFeature()->getCapture();
}
void FactorBase::setStatus(FactorStatus _status)
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index a4f361000..5836bab02 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -67,9 +67,9 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
return _co_ptr;
}
-FrameBasePtr FeatureBase::getFramePtr() const
+FrameBasePtr FeatureBase::getFrame() const
{
- return capture_ptr_.lock()->getFramePtr();
+ return capture_ptr_.lock()->getFrame();
}
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _ctr_ptr)
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 48d0351fc..3e983ef3e 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -71,8 +71,8 @@ void FrameBase::remove()
if ( isKey() )
removeStateBlocks();
- if (getTrajectoryPtr()->getLastKeyFramePtr()->id() == this_F->id())
- getTrajectoryPtr()->setLastKeyFramePtr(getTrajectoryPtr()->findLastKeyFramePtr());
+ if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
+ getTrajectory()->setLastKeyFramePtr(getTrajectory()->findLastKeyFrame());
// std::cout << "Removed F" << id() << std::endl;
}
@@ -81,8 +81,8 @@ void FrameBase::remove()
void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
- if (isKey() && getTrajectoryPtr() != nullptr)
- getTrajectoryPtr()->sortFrame(shared_from_this());
+ if (isKey() && getTrajectory() != nullptr)
+ getTrajectory()->sortFrame(shared_from_this());
}
void FrameBase::registerNewStateBlocks()
@@ -118,10 +118,10 @@ void FrameBase::setKey()
type_ = KEY_FRAME;
registerNewStateBlocks();
- if (getTrajectoryPtr()->getLastKeyFramePtr() == nullptr || getTrajectoryPtr()->getLastKeyFramePtr()->getTimeStamp() < time_stamp_)
- getTrajectoryPtr()->setLastKeyFramePtr(shared_from_this());
+ if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
+ getTrajectory()->setLastKeyFramePtr(shared_from_this());
- getTrajectoryPtr()->sortFrame(shared_from_this());
+ getTrajectory()->sortFrame(shared_from_this());
// WOLF_DEBUG("Set KF", this->id());
}
@@ -229,18 +229,18 @@ Eigen::MatrixXs FrameBase::getCovariance() const
FrameBasePtr FrameBase::getPreviousFrame() const
{
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
- if (getTrajectoryPtr() == nullptr)
+ if (getTrajectory() == nullptr)
//std::cout << "This Frame is not linked to any trajectory" << std::endl;
- assert(getTrajectoryPtr() != nullptr && "This Frame is not linked to any trajectory");
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
- for (auto f_it = getTrajectoryPtr()->getFrameList().rbegin(); f_it != getTrajectoryPtr()->getFrameList().rend(); f_it++ )
+ for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
{
if ( this->frame_id_ == (*f_it)->id() )
{
f_it++;
- if (f_it != getTrajectoryPtr()->getFrameList().rend())
+ if (f_it != getTrajectory()->getFrameList().rend())
{
//std::cout << "previous frame found!" << std::endl;
return *f_it;
@@ -259,11 +259,11 @@ FrameBasePtr FrameBase::getPreviousFrame() const
FrameBasePtr FrameBase::getNextFrame() const
{
//std::cout << "finding next frame of " << this->frame_id_ << std::endl;
- auto f_it = getTrajectoryPtr()->getFrameList().rbegin();
+ auto f_it = getTrajectory()->getFrameList().rbegin();
f_it++; //starting from second last frame
//look for the position of this node in the frame list of trajectory
- while (f_it != getTrajectoryPtr()->getFrameList().rend())
+ while (f_it != getTrajectory()->getFrameList().rend())
{
if ( this->frame_id_ == (*f_it)->id())
{
@@ -288,7 +288,7 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
{
for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensorPtr() == _sensor_ptr && capture_ptr->getType() == type)
+ if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
@@ -296,7 +296,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr)
{
for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensorPtr() == _sensor_ptr)
+ if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
return nullptr;
}
@@ -305,7 +305,7 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
{
CaptureBasePtrList captures;
for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensorPtr() == _sensor_ptr)
+ if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
return captures;
}
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 17a22bac3..1daa37470 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -37,7 +37,7 @@ YAML::Node LandmarkAHP::saveToYaml() const
Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
{
- Eigen::Map<const Eigen::Vector4s> hmg_point(getPPtr()->getState().data());
+ Eigen::Map<const Eigen::Vector4s> hmg_point(getP()->getState().data());
return hmg_point.head<3>()/hmg_point(3);
}
@@ -45,12 +45,12 @@ Eigen::Vector3s LandmarkAHP::point() const
{
using namespace Eigen;
Transform<Scalar,3,Affine> T_w_r
- = Translation<Scalar,3>(getAnchorFrame()->getPPtr()->getState())
- * Quaternions(getAnchorFrame()->getOPtr()->getState().data());
+ = Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
+ * Quaternions(getAnchorFrame()->getO()->getState().data());
Transform<Scalar,3,Affine> T_r_c
- = Translation<Scalar,3>(getAnchorSensor()->getPPtr()->getState())
- * Quaternions(getAnchorSensor()->getOPtr()->getState().data());
- Vector4s point_hmg_c = getPPtr()->getState();
+ = Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
+ * Quaternions(getAnchorSensor()->getO()->getState().data());
+ Vector4s point_hmg_c = getP()->getState();
Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
return point_hmg.head<3>()/point_hmg(3);
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index f2eb27e71..32bf0b6a7 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -157,15 +157,15 @@ YAML::Node LandmarkBase::saveToYaml() const
YAML::Node node;
node["id"] = landmark_id_;
node["type"] = node_type_;
- if (getPPtr() != nullptr)
+ if (getP() != nullptr)
{
- node["position"] = getPPtr()->getState();
- node["position fixed"] = getPPtr()->isFixed();
+ node["position"] = getP()->getState();
+ node["position fixed"] = getP()->isFixed();
}
- if (getOPtr() != nullptr)
+ if (getO() != nullptr)
{
- node["orientation"] = getOPtr()->getState();
- node["orientation fixed"] = getOPtr()->isFixed();
+ node["orientation"] = getO()->getState();
+ node["orientation fixed"] = getO()->isFixed();
}
return node;
}
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
index 6cef90a38..3a424869b 100644
--- a/src/landmark/landmark_container.cpp
+++ b/src/landmark/landmark_container.cpp
@@ -31,8 +31,8 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// Computing center
WOLF_DEBUG( "Container constructor: Computing center pose... " );
- Eigen::Vector2s container_position(getPPtr()->getState());
- Eigen::Vector1s container_orientation(getOPtr()->getState());
+ Eigen::Vector2s container_position(getP()->getState());
+ Eigen::Vector1s container_orientation(getO()->getState());
WOLF_DEBUG( "Container constructor: _corner_1_idx ", _corner_1_idx,
"_corner_2_idx ", _corner_2_idx );
@@ -83,8 +83,8 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
WOLF_DEBUG( "_corner_2_pose... ", _corner_2_pose.transpose() );
WOLF_DEBUG( "Container center pose... ", container_position.transpose(), " ", container_orientation.transpose() );
- getPPtr()->setState(container_position);
- getOPtr()->setState(container_orientation);
+ getP()->setState(container_position);
+ getO()->setState(container_orientation);
}
//LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :
@@ -103,8 +103,8 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
//
// // Computing center
// //std::cout << "Container constructor: Computing center position... " << std::endl;
-// Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->getPtr());
-// Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->getPtr(), _o_ptr->getSize());
+// Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->get());
+// Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->get(), _o_ptr->getSize());
//
// container_position = Eigen::Vector2s::Zero();
//
@@ -112,53 +112,53 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// // A & B
// if (_corner_A_ptr != nullptr && _corner_B_ptr != nullptr)
// {
-// Eigen::Vector2s AB = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s AB = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
// Eigen::Vector2s perpendicularAB;
// perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) + AB / 2 + perpendicularAB * _witdh / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AB / 2 + perpendicularAB * _witdh / 2;
// container_orientation(0) = atan2(AB(1),AB(0));
// }
// // C & D
// else if (_corner_C_ptr != nullptr && _corner_D_ptr != nullptr)
// {
-// Eigen::Vector2s CD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s CD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get());
// Eigen::Vector2s perpendicularCD;
// perpendicularCD << -CD(1)/CD.norm(), CD(0)/CD.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) + CD / 2 + perpendicularCD * _witdh / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) + CD / 2 + perpendicularCD * _witdh / 2;
// container_orientation(0) = atan2(-CD(1),-CD(0));
// }
// // Short side detected
// // B & C
// else if (_corner_B_ptr != nullptr && _corner_C_ptr != nullptr)
// {
-// Eigen::Vector2s BC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s BC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
// Eigen::Vector2s perpendicularBC;
// perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) + BC / 2 + perpendicularBC * _length / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BC / 2 + perpendicularBC * _length / 2;
// container_orientation(0) = atan2(BC(1),BC(0));
// }
// // D & A
// else if (_corner_D_ptr != nullptr && _corner_A_ptr != nullptr)
// {
-// Eigen::Vector2s DA = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s DA = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get());
// Eigen::Vector2s perpendicularDA;
// perpendicularDA << -DA(1)/DA.norm(), DA(0)/DA.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) + DA / 2 + perpendicularDA * _length / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) + DA / 2 + perpendicularDA * _length / 2;
// container_orientation(0) = atan2(-DA(1),-DA(0));
// }
// // Diagonal detected
// // A & C
// else if (_corner_A_ptr != nullptr && _corner_C_ptr != nullptr)
// {
-// Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) + AC / 2;
+// Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AC / 2;
// container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
// }
// // B & D
// else if (_corner_B_ptr != nullptr && _corner_D_ptr != nullptr)
// {
-// Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) + BD / 2;
+// Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BD / 2;
// container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
// }
//}
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 9e9ccf664..91e790ae7 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -248,7 +248,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// If landmark point constrained -> new factor
if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
- new_ctr_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+ new_ctr_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
ctr_point_ptr->getProcessor(),
ctr_point_ptr->getFeaturePointId(),
@@ -263,7 +263,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// If landmark point constrained -> new factor
if (ctr_point_line_ptr->getLandmarkPointId() == _remove_id)
- new_ctr_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+ new_ctr_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
ctr_point_line_ptr->getProcessor(),
ctr_point_line_ptr->getFeaturePointId(),
@@ -273,7 +273,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
ctr_point_line_ptr->getStatus());
// If landmark point is aux point -> new factor
else if (ctr_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
- new_ctr_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+ new_ctr_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
ctr_point_line_ptr->getProcessor(),
ctr_point_line_ptr->getFeaturePointId(),
@@ -292,7 +292,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
std::cout << "deleting factor: " << ctr_ptr->id() << std::endl;
// add new factor
- ctr_ptr->getFeaturePtr()->addFactor(new_ctr_ptr);
+ ctr_ptr->getFeature()->addFactor(new_ctr_ptr);
// remove factor
ctr_ptr->remove();
diff --git a/src/problem.cpp b/src/problem.cpp
index 10a377aa3..c410886af 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -79,7 +79,7 @@ Problem::~Problem()
void Problem::addSensor(SensorBasePtr _sen_ptr)
{
- getHardwarePtr()->addSensor(_sen_ptr);
+ getHardware()->addSensor(_sen_ptr);
}
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
@@ -127,7 +127,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFramePtr());
+ (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
@@ -156,14 +156,14 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
{
- auto sen_it = std::find_if(getHardwarePtr()->getSensorList().begin(),
- getHardwarePtr()->getSensorList().end(),
+ auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
+ getHardware()->getSensorList().end(),
[&](SensorBasePtr sb)
{
return sb->getName() == _sensor_name;
}); // lambda function for the find_if
- if (sen_it == getHardwarePtr()->getSensorList().end())
+ if (sen_it == getHardware()->getSensorList().end())
return nullptr;
return (*sen_it);
@@ -171,7 +171,7 @@ SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
- for (auto sen : getHardwarePtr()->getSensorList()) // loop all sensors
+ for (auto sen : getHardware()->getSensorList()) // loop all sensors
{
auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
sen->getProcessorList().end(),
@@ -253,8 +253,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(state);
- else if (trajectory_ptr_->getLastKeyFramePtr() != nullptr)
- trajectory_ptr_->getLastKeyFramePtr()->getState(state);
+ else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
+ trajectory_ptr_->getLastKeyFrame()->getState(state);
else
state = zeroState();
}
@@ -268,10 +268,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
processor_motion_ptr_->getCurrentState(state);
processor_motion_ptr_->getCurrentTimeStamp(ts);
}
- else if (trajectory_ptr_->getLastKeyFramePtr() != nullptr)
+ else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
{
- trajectory_ptr_->getLastKeyFramePtr()->getTimeStamp(ts);
- trajectory_ptr_->getLastKeyFramePtr()->getState(state);
+ trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts);
+ trajectory_ptr_->getLastKeyFrame()->getState(state);
}
else
state = zeroState();
@@ -356,13 +356,13 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
{
- getMapPtr()->addLandmark(_lmk_ptr);
+ getMap()->addLandmark(_lmk_ptr);
return _lmk_ptr;
}
void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
{
- getMapPtr()->addLandmarkList(_lmk_list);
+ getMap()->addLandmarkList(_lmk_list);
}
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
@@ -598,7 +598,7 @@ Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr)
Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
{
- FrameBasePtr frm = getLastKeyFramePtr();
+ FrameBasePtr frm = getLastKeyFrame();
return getFrameCovariance(frm);
}
@@ -641,29 +641,29 @@ Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_pt
return covariance;
}
-MapBasePtr Problem::getMapPtr()
+MapBasePtr Problem::getMap()
{
return map_ptr_;
}
-TrajectoryBasePtr Problem::getTrajectoryPtr()
+TrajectoryBasePtr Problem::getTrajectory()
{
return trajectory_ptr_;
}
-HardwareBasePtr Problem::getHardwarePtr()
+HardwareBasePtr Problem::getHardware()
{
return hardware_ptr_;
}
-FrameBasePtr Problem::getLastFramePtr()
+FrameBasePtr Problem::getLastFrame()
{
- return trajectory_ptr_->getLastFramePtr();
+ return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::getLastKeyFramePtr()
+FrameBasePtr Problem::getLastKeyFrame()
{
- return trajectory_ptr_->getLastKeyFramePtr();
+ return trajectory_ptr_->getLastKeyFrame();
}
StateBlockPtrList& Problem::getStateBlockPtrList()
@@ -714,12 +714,12 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
void Problem::loadMap(const std::string& _filename_dot_yaml)
{
- getMapPtr()->load(_filename_dot_yaml);
+ getMap()->load(_filename_dot_yaml);
}
void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
{
- getMapPtr()->save(_filename_dot_yaml, _map_name);
+ getMap()->save(_filename_dot_yaml, _map_name);
}
void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
@@ -729,11 +729,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl;
cout << "P: wolf tree status ---------------------" << endl;
- cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardwarePtr()->getSensorList().size()) + "S") : "") << endl;
+ cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
if (depth >= 1)
{
// Sensors =======================================================================================
- for (auto S : getHardwarePtr()->getSensorList())
+ for (auto S : getHardware()->getSensorList())
{
cout << " S" << S->id() << " " << S->getType();
if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
@@ -758,13 +758,13 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
else if (metric)
{
cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getPPtr())
- cout << S->getPPtr()->getState().transpose();
- if (S->getOPtr())
- cout << " " << S->getOPtr()->getState().transpose();
+ if (S->getP())
+ cout << S->getP()->getState().transpose();
+ if (S->getO())
+ cout << " " << S->getO()->getState().transpose();
cout << " )";
- if (S->getIntrinsicPtr())
- cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsicPtr()->getState().transpose() << " )";
+ if (S->getIntrinsic())
+ cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
cout << endl;
}
else if (state_blocks)
@@ -784,14 +784,14 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
std::cout << " pm" << p->id() << " " << p->getType() << endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOriginPtr())
- cout << " o: C" << pm->getOriginPtr()->id() << " - " << (pm->getOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pm->getOriginPtr()->getFramePtr()->id() << endl;
- if (pm->getLastPtr())
- cout << " l: C" << pm->getLastPtr()->id() << " - " << (pm->getLastPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pm->getLastPtr()->getFramePtr()->id() << endl;
- if (pm->getIncomingPtr())
- cout << " i: C" << pm->getIncomingPtr()->id() << endl;
+ if (pm->getOrigin())
+ cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKey() ? " KF" : " F")
+ << pm->getOrigin()->getFrame()->id() << endl;
+ if (pm->getLast())
+ cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKey() ? " KF" : " F")
+ << pm->getLast()->getFrame()->id() << endl;
+ if (pm->getIncoming())
+ cout << " i: C" << pm->getIncoming()->id() << endl;
}
else
{
@@ -802,29 +802,29 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
// if (ptt)
// {
-// if (ptt->getPrevOriginPtr())
-// cout << " p: C" << ptt->getPrevOriginPtr()->id() << " - " << (ptt->getPrevOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
-// << ptt->getPrevOriginPtr()->getFramePtr()->id() << endl;
+// if (ptt->getPrevOrigin())
+// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " F")
+// << ptt->getPrevOrigin()->getFrame()->id() << endl;
// }
- if (pt->getOriginPtr())
- cout << " o: C" << pt->getOriginPtr()->id() << " - " << (pt->getOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pt->getOriginPtr()->getFramePtr()->id() << endl;
- if (pt->getLastPtr())
- cout << " l: C" << pt->getLastPtr()->id() << " - " << (pt->getLastPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pt->getLastPtr()->getFramePtr()->id() << endl;
- if (pt->getIncomingPtr())
- cout << " i: C" << pt->getIncomingPtr()->id() << endl;
+ if (pt->getOrigin())
+ cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKey() ? " KF" : " F")
+ << pt->getOrigin()->getFrame()->id() << endl;
+ if (pt->getLast())
+ cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKey() ? " KF" : " F")
+ << pt->getLast()->getFrame()->id() << endl;
+ if (pt->getIncoming())
+ cout << " i: C" << pt->getIncoming()->id() << endl;
}
}
} // for p
}
} // for S
}
- cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectoryPtr()->getFrameList().size()) + "F") : "") << endl;
+ cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
if (depth >= 1)
{
// Frames =======================================================================================
- for (auto F : getTrajectoryPtr()->getFrameList())
+ for (auto F : getTrajectory()->getFrameList())
{
cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
if (constr_by)
@@ -856,19 +856,19 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
- if(C->getSensorPtr() != nullptr)
+ if(C->getSensor() != nullptr)
{
- cout << " -> S" << C->getSensorPtr()->id();
- cout << (C->getSensorPtr()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- cout << (C->getSensorPtr()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
+ cout << " -> S" << C->getSensor()->id();
+ cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
+ cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
}
else
cout << " -> S-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (CM->getOriginFramePtr())
- cout << " -> OF" << CM->getOriginFramePtr()->id();
+ if (CM->getOriginFrame())
+ cout << " -> OF" << CM->getOriginFrame()->id();
}
cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
@@ -930,16 +930,16 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
for (auto c : f->getFactorList())
{
cout << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOtherPtr() == nullptr && c->getCaptureOtherPtr() == nullptr && c->getFeatureOtherPtr() == nullptr && c->getLandmarkOtherPtr() == nullptr)
+ if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
cout << " A";
- if (c->getFrameOtherPtr() != nullptr)
- cout << " F" << c->getFrameOtherPtr()->id();
- if (c->getCaptureOtherPtr() != nullptr)
- cout << " C" << c->getCaptureOtherPtr()->id();
- if (c->getFeatureOtherPtr() != nullptr)
- cout << " f" << c->getFeatureOtherPtr()->id();
- if (c->getLandmarkOtherPtr() != nullptr)
- cout << " L" << c->getLandmarkOtherPtr()->id();
+ if (c->getFrameOther() != nullptr)
+ cout << " F" << c->getFrameOther()->id();
+ if (c->getCaptureOther() != nullptr)
+ cout << " C" << c->getCaptureOther()->id();
+ if (c->getFeatureOther() != nullptr)
+ cout << " f" << c->getFeatureOther()->id();
+ if (c->getLandmarkOther() != nullptr)
+ cout << " L" << c->getLandmarkOther()->id();
cout << endl;
} // for c
}
@@ -949,11 +949,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
} // for F
}
- cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMapPtr()->getLandmarkList().size()) + "L") : "") << endl;
+ cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
if (depth >= 1)
{
// Landmarks =======================================================================================
- for (auto L : getMapPtr()->getLandmarkList())
+ for (auto L : getMap()->getLandmarkList())
{
cout << " L" << L->id() << " " << L->getType();
if (constr_by)
@@ -1020,13 +1020,13 @@ bool Problem::check(int verbose_level)
{
cout << " S" << S->id() << " @ " << S.get() << endl;
cout << " -> P @ " << S->getProblem().get() << endl;
- cout << " -> H @ " << S->getHardwarePtr().get() << endl;
+ cout << " -> H @ " << S->getHardware().get() << endl;
for (auto sb : S->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1035,20 +1035,20 @@ bool Problem::check(int verbose_level)
}
// check problem and hardware pointers
is_consistent = is_consistent && (S->getProblem().get() == P_raw);
- is_consistent = is_consistent && (S->getHardwarePtr() == H);
+ is_consistent = is_consistent && (S->getHardware() == H);
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (verbose_level > 0)
{
- cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensorPtr()->id() << endl;
+ cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
cout << " -> P @ " << p->getProblem().get() << endl;
- cout << " -> S" << p->getSensorPtr()->id() << " @ " << p->getSensorPtr().get() << endl;
+ cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl;
}
// check problem and sensor pointers
is_consistent = is_consistent && (p->getProblem().get() == P_raw);
- is_consistent = is_consistent && (p->getSensorPtr() == S);
+ is_consistent = is_consistent && (p->getSensor() == S);
}
}
// ------------------------
@@ -1069,13 +1069,13 @@ bool Problem::check(int verbose_level)
{
cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << endl;
cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectoryPtr().get() << endl;
+ cout << " -> T @ " << F->getTrajectory().get() << endl;
for (auto sb : F->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1084,15 +1084,15 @@ bool Problem::check(int verbose_level)
}
// check problem and trajectory pointers
is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectoryPtr() == T);
+ is_consistent = is_consistent && (F->getTrajectory() == T);
for (auto cby : F->getConstrainedByList())
{
if (verbose_level > 0)
{
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOtherPtr()->id() << endl;
+ cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
}
// check constrained_by pointer to this frame
- is_consistent = is_consistent && (cby->getFrameOtherPtr() == F);
+ is_consistent = is_consistent && (cby->getFrameOther() == F);
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose_level > 0)
@@ -1100,7 +1100,7 @@ bool Problem::check(int verbose_level)
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1115,17 +1115,17 @@ bool Problem::check(int verbose_level)
if (verbose_level > 0)
{
cout << " C" << C->id() << " @ " << C.get() << " -> S";
- if (C->getSensorPtr()) cout << C->getSensorPtr()->id();
+ if (C->getSensor()) cout << C->getSensor()->id();
else cout << "-";
cout << endl;
cout << " -> P @ " << C->getProblem().get() << endl;
- cout << " -> F" << C->getFramePtr()->id() << " @ " << C->getFramePtr().get() << endl;
+ cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
for (auto sb : C->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1134,7 +1134,7 @@ bool Problem::check(int verbose_level)
}
// check problem and frame pointers
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
- is_consistent = is_consistent && (C->getFramePtr() == F);
+ is_consistent = is_consistent && (C->getFrame() == F);
// Features =======================================================================================
for (auto f : C->getFeatureList())
@@ -1143,21 +1143,21 @@ bool Problem::check(int verbose_level)
{
cout << " f" << f->id() << " @ " << f.get() << endl;
cout << " -> P @ " << f->getProblem().get() << endl;
- cout << " -> C" << f->getCapturePtr()->id() << " @ " << f->getCapturePtr().get()
+ cout << " -> C" << f->getCapture()->id() << " @ " << f->getCapture().get()
<< endl;
}
// check problem and capture pointers
is_consistent = is_consistent && (f->getProblem().get() == P_raw);
- is_consistent = is_consistent && (f->getCapturePtr() == C);
+ is_consistent = is_consistent && (f->getCapture() == C);
for (auto cby : f->getConstrainedByList())
{
if (verbose_level > 0)
{
- cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOtherPtr()->id() << endl;
+ cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
}
// check constrained_by pointer to this feature
- is_consistent = is_consistent && (cby->getFeatureOtherPtr() == f);
+ is_consistent = is_consistent && (cby->getFeatureOther() == f);
}
// Factors =======================================================================================
@@ -1166,10 +1166,10 @@ bool Problem::check(int verbose_level)
if (verbose_level > 0)
cout << " c" << c->id() << " @ " << c.get();
- auto Fo = c->getFrameOtherPtr();
- auto Co = c->getCaptureOtherPtr();
- auto fo = c->getFeatureOtherPtr();
- auto Lo = c->getLandmarkOtherPtr();
+ auto Fo = c->getFrameOther();
+ auto Co = c->getCaptureOther();
+ auto fo = c->getFeatureOther();
+ auto Lo = c->getLandmarkOther();
if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
{
@@ -1251,14 +1251,14 @@ bool Problem::check(int verbose_level)
if (verbose_level > 0)
{
cout << " -> P @ " << c->getProblem().get() << endl;
- cout << " -> f" << c->getFeaturePtr()->id() << " @ " << c->getFeaturePtr().get() << endl;
+ cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
}
// check problem and feature pointers
is_consistent = is_consistent && (c->getProblem().get() == P_raw);
- is_consistent = is_consistent && (c->getFeaturePtr() == f);
+ is_consistent = is_consistent && (c->getFeature() == f);
// find state block pointers in all constrained nodes
- SensorBasePtr S = c->getFeaturePtr()->getCapturePtr()->getSensorPtr(); // get own sensor to check sb
+ SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
@@ -1267,7 +1267,7 @@ bool Problem::check(int verbose_level)
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1289,13 +1289,13 @@ bool Problem::check(int verbose_level)
if (fo)
{
// find in constrained feature's Frame
- FrameBasePtr foF = fo->getFramePtr();
+ FrameBasePtr foF = fo->getFrame();
found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
// find in constrained feature's Capture
- CaptureBasePtr foC = fo->getCapturePtr();
+ CaptureBasePtr foC = fo->getCapture();
found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
// find in constrained feature's Sensor
- SensorBasePtr foS = fo->getCapturePtr()->getSensorPtr();
+ SensorBasePtr foS = fo->getCapture()->getSensor();
found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
}
// find in constrained landmark
@@ -1332,13 +1332,13 @@ bool Problem::check(int verbose_level)
{
cout << " L" << L->id() << " @ " << L.get() << endl;
cout << " -> P @ " << L->getProblem().get() << endl;
- cout << " -> M @ " << L->getMapPtr().get() << endl;
+ cout << " -> M @ " << L->getMap().get() << endl;
for (auto sb : L->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1347,13 +1347,13 @@ bool Problem::check(int verbose_level)
}
// check problem and map pointers
is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- is_consistent = is_consistent && (L->getMapPtr() == M);
+ is_consistent = is_consistent && (L->getMap() == M);
for (auto cby : L->getConstrainedByList())
{
if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOtherPtr()->id() << endl;
+ cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
// check constrained_by pointer to this landmark
- is_consistent = is_consistent && (cby->getLandmarkOtherPtr() && L->id() == cby->getLandmarkOtherPtr()->id());
+ is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose_level > 0)
@@ -1361,7 +1361,7 @@ bool Problem::check(int verbose_level)
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index d179060bd..3db871753 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -219,7 +219,7 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
// link ot wolf tree
_feature_motion->addFactor(ctr_imu);
_capture_origin->addConstrainedBy(ctr_imu);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_imu);
+ _capture_origin->getFrame()->addConstrainedBy(ctr_imu);
return ctr_imu;
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 60569422e..82fa41c13 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -62,7 +62,7 @@ void ProcessorBase::remove()
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && P->getProcessorMotionPtr()->id() == this->id())
+ if(P && P->getProcessorMotion()->id() == this->id())
P->clearProcessorMotion();
}
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
index 58e3550ab..2958739d7 100644
--- a/src/processor/processor_capture_holder.cpp
+++ b/src/processor/processor_capture_holder.cpp
@@ -26,7 +26,7 @@ void ProcessorCaptureHolder::process(CaptureBasePtr _capture_ptr)
void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
const Scalar& _time_tolerance)
{
- assert(_keyframe_ptr->getTrajectoryPtr() != nullptr
+ assert(_keyframe_ptr->getTrajectory() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
// get keyframe's time stamp
@@ -48,7 +48,7 @@ void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
// add motion capture to keyframe
if (std::abs(new_ts - existing_capture->getTimeStamp()) < _time_tolerance)
{
- auto frame_ptr = existing_capture->getFramePtr();
+ auto frame_ptr = existing_capture->getFrame();
if (frame_ptr != _keyframe_ptr)
{
@@ -92,11 +92,11 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
// // go to the previous motion capture
// if (capture_ptr == last_ptr_)
// capture_ptr = origin_ptr_;
-// else if (capture_ptr->getOriginFramePtr() == nullptr)
+// else if (capture_ptr->getOriginFrame() == nullptr)
// return nullptr;
// else
// {
-// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFrame()->getCaptureOf(getSensor());
// if (capture_base_ptr == nullptr)
// return nullptr;
// else
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index a5a41f3fa..07b577b6d 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -34,7 +34,7 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
/// Retrieve sensor intrinsics
const IntrinsicsDiffDrive& intrinsics =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
+ *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
VelocityComand<Scalar> vel;
Eigen::MatrixXs J_vel_data;
@@ -213,11 +213,11 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin)
{
FactorOdom2DPtr ctr_odom =
- std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFramePtr(),
+ std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
shared_from_this());
_feature->addFactor(ctr_odom);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
+ _capture_origin->getFrame()->addConstrainedBy(ctr_odom);
return ctr_odom;
}
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 33cbd2c55..7df561f9c 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -58,11 +58,11 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
const ProblemPtr problem_ptr = getProblem();
const std::string frame_structure =
- problem_ptr->getTrajectoryPtr()->getFrameStructure();
+ problem_ptr->getTrajectory()->getFrameStructure();
// get the list of all frames
const FrameBasePtrList& frame_list = problem_ptr->
- getTrajectoryPtr()->
+ getTrajectory()->
getFrameList();
bool found_possible_candidate = false;
@@ -72,7 +72,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
// check for LC just if frame is key frame
// Assert that the evaluated KF has a capture of the
// same sensor as this processor
- if (key_it->isKey() && key_it->getCaptureOf(getSensorPtr()/*, "LASER 2D"*/) != nullptr)
+ if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
{
// Check if the two frames currently evaluated are already
// constrained one-another.
@@ -82,7 +82,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
for (const FactorBasePtr& crt : ctr_list)
{
// Are the two frames constrained one-another ?
- if (crt->getFrameOtherPtr() == problem_ptr->getLastKeyFramePtr())
+ if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
{
// By this very processor ?
if (crt->getProcessor() == shared_from_this())
@@ -127,7 +127,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
Eigen::Vector5s frame_covariance, current_covariance;
if (!computeEllipse2D(key_it,
frame_covariance)) continue;
- if (!computeEllipse2D(getProblem()->getLastKeyFramePtr(),
+ if (!computeEllipse2D(getProblem()->getLastKeyFrame(),
current_covariance)) continue;
found_possible_candidate = ellipse2DIntersect(frame_covariance,
current_covariance);
@@ -160,7 +160,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
Eigen::Vector10s frame_covariance, current_covariance;
if (!computeEllipsoid3D(key_it,
frame_covariance)) continue;
- if (!computeEllipsoid3D(getProblem()->getLastKeyFramePtr(),
+ if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(),
frame_covariance)) continue;
found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
current_covariance);
@@ -170,7 +170,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
{
found_possible_candidate = insideMahalanisDistance(key_it,
- problem_ptr->getLastKeyFramePtr());
+ problem_ptr->getLastKeyFrame());
break;
}
@@ -210,7 +210,7 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& f
}
// get position of frame AKA mean [x, y]
- const Eigen::VectorXs frame_position = frame_ptr->getPPtr()->getState();
+ const Eigen::VectorXs frame_position = frame_ptr->getP()->getState();
ellipse(0) = frame_position(0);
ellipse(1) = frame_position(1);
@@ -239,7 +239,7 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr&
// Ellipse description [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
// get position of frame AKA mean [x, y, z]
- const Eigen::VectorXs frame_position = frame_pointer->getPPtr()->getState();
+ const Eigen::VectorXs frame_position = frame_pointer->getP()->getState();
ellipsoid(0) = frame_position(0);
ellipsoid(1) = frame_position(1);
ellipsoid(2) = frame_position(2);
@@ -447,7 +447,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
Eigen::VectorXs traj_pose, query_pose;
// get state and covariance matrix for both frames
- if (trajectory->getPPtr()->getState().size() == 2)
+ if (trajectory->getP()->getState().size() == 2)
{
traj_pose.resize(3);
query_pose.resize(3);
@@ -458,11 +458,11 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
query_pose.resize(7);
}
- traj_pose << trajectory->getPPtr()->getState(),
- trajectory->getOPtr()->getState();
+ traj_pose << trajectory->getP()->getState(),
+ trajectory->getO()->getState();
- query_pose << query->getPPtr()->getState(),
- query->getOPtr()->getState();
+ query_pose << query->getP()->getState(),
+ query->getO()->getState();
const Eigen::MatrixXs traj_covariance = getProblem()->getFrameCovariance(trajectory);
const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query);
@@ -482,7 +482,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
//##############################################################################
bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
{
- FrameBasePtr keyframe = getProblem()->getLastKeyFramePtr();
+ FrameBasePtr keyframe = getProblem()->getLastKeyFrame();
if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
return false;
else
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index a2765043b..f20aec34d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -102,11 +102,11 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
auto existing_capture = findCaptureContainingTimeStamp(ts_from_callback);
// Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFramePtr();
+ auto keyframe_origin = existing_capture->getOriginFrame();
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensorPtr(),
+ getSensor(),
ts_from_callback,
Eigen::VectorXs::Zero(data_size_),
existing_capture->getDataCovariance(),
@@ -122,7 +122,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
auto new_feature = emplaceFeature(capture_for_keyframe_callback);
// create motion factor and add it to the feature, and constrain to the other capture (origin)
- emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
+ emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensor()) );
// modify existing feature and factor (if they exist in the existing capture)
if (!existing_capture->getFeatureList().empty())
@@ -149,12 +149,12 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
// Find the frame acting as the capture's origin
- auto keyframe_origin = last_ptr_->getOriginFramePtr();
+ auto keyframe_origin = last_ptr_->getOriginFrame();
// emplace a new motion capture to the new keyframe
VectorXs calib = last_ptr_->getCalibration();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensorPtr(),
+ getSensor(),
ts_from_callback,
Eigen::VectorXs::Zero(data_size_),
last_ptr_->getDataCovariance(),
@@ -170,7 +170,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
// create motion factor and add it to the feature, and constrain to the other capture (origin)
- emplaceFactor(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensorPtr()) );
+ emplaceFactor(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensor()) );
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
@@ -190,13 +190,13 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFramePtr()->setState(getCurrentState());
+ last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
+ last_ptr_->getFrame()->setState(getCurrentState());
if (voteForKeyFrame() && permittedKeyFrame())
{
// Set the frame of last_ptr as key
- auto key_frame_ptr = last_ptr_->getFramePtr();
+ auto key_frame_ptr = last_ptr_->getFrame();
key_frame_ptr->setKey();
// create motion feature and add it to the key_capture
@@ -211,7 +211,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
getCurrentTimeStamp());
// create a new capture
auto new_capture_ptr = emplaceCapture(new_frame_ptr,
- getSensorPtr(),
+ getSensor(),
key_frame_ptr->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
@@ -260,8 +260,8 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
{
// Get origin state and calibration
- VectorXs state_0 = capture_motion->getOriginFramePtr()->getState();
- CaptureBasePtr cap_orig = capture_motion->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+ VectorXs state_0 = capture_motion->getOriginFrame()->getState();
+ CaptureBasePtr cap_orig = capture_motion->getOriginFrame()->getCaptureOf(getSensor());
VectorXs calib = cap_orig->getCalibration();
// Get delta and correct it with new calibration params
@@ -273,7 +273,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
// Compose on top of origin state using the buffered time stamp, not the query time stamp
// TODO Interpolate the delta to produce a state at the query time stamp _ts
- Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOriginPtr()->getTimeStamp();
+ Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOrigin()->getTimeStamp();
statePlusDelta(state_0, delta, dt, _x);
}
else
@@ -297,19 +297,19 @@ FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const
void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
{
assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
- assert(_origin_frame->getTrajectoryPtr() != nullptr
+ assert(_origin_frame->getTrajectory() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
// -------- ORIGIN ---------
// emplace (empty) origin Capture
origin_ptr_ = emplaceCapture(_origin_frame,
- getSensorPtr(),
+ getSensor(),
_origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensorPtr()->getCalibration(),
- getSensorPtr()->getCalibration(),
+ getSensor()->getCalibration(),
+ getSensor()->getCalibration(),
nullptr);
// ---------- LAST ----------
@@ -319,12 +319,12 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
_origin_frame->getTimeStamp());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
- getSensorPtr(),
+ getSensor(),
_origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensorPtr()->getCalibration(),
- getSensorPtr()->getCalibration(),
+ getSensor()->getCalibration(),
+ getSensor()->getCalibration(),
_origin_frame);
// clear and reset buffer
@@ -394,7 +394,7 @@ void ProcessorMotion::integrateOneStep()
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
{
// start with empty motion
- _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFramePtr()->getTimeStamp()));
+ _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
VectorXs calib = _capture_ptr->getCalibrationPreint();
@@ -534,12 +534,12 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectoryPtr()->getFrameList().rbegin();
- frame_rev_iter != getProblem()->getTrajectoryPtr()->getFrameList().rend();
+ for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
+ frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
++frame_rev_iter)
{
frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensorPtr());
+ capture = frame->getCaptureOf(getSensor());
if (capture != nullptr)
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 2f88eeffb..57119faf6 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -136,8 +136,8 @@ bool ProcessorOdom2D::voteForKeyFrame()
return true;
}
// Time criterion
- WOLF_TRACE("orig t: ", origin_ptr_->getFramePtr()->getTimeStamp().get(), " current t: ", getBuffer().get().back().ts_.get() , " dt: ", getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get());
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get() > params_odom_2D_->max_time_span)
+ WOLF_TRACE("orig t: ", origin_ptr_->getFrame()->getTimeStamp().get(), " current t: ", getBuffer().get().back().ts_.get() , " dt: ", getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get());
+ if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2D_->max_time_span)
{
return true;
}
@@ -154,10 +154,10 @@ CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const Sens
FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- FactorOdom2DPtr ctr_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFramePtr(),
+ FactorOdom2DPtr ctr_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
shared_from_this());
_feature->addFactor(ctr_odom);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
+ _capture_origin->getFrame()->addConstrainedBy(ctr_odom);
return ctr_odom;
}
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index a09c4fa5e..917ee944a 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -405,10 +405,10 @@ CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const Sens
FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- FactorOdom3DPtr ctr_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFramePtr(),
+ FactorOdom3DPtr ctr_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
shared_from_this());
_feature_motion->addFactor(ctr_odom);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
+ _capture_origin->getFrame()->addConstrainedBy(ctr_odom);
return ctr_odom;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 5e66e2054..e502ba296 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -131,7 +131,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
processKnown();
// Capture last_ is added to the new keyframe
- FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
+ FrameBasePtr last_old_frame = last_ptr_->getFrame();
last_old_frame->unlinkCapture(last_ptr_);
last_old_frame->remove();
pack->key_frame->addCapture(last_ptr_);
@@ -170,7 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We create a KF
// set KF on last
- last_ptr_->getFramePtr()->setKey();
+ last_ptr_->getFrame()->setKey();
// make F; append incoming to new F
FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
@@ -180,16 +180,16 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
processNew(params_tracker_->max_new_features);
// // Set key
- // last_ptr_->getFramePtr()->setKey();
+ // last_ptr_->getFrame()->setKey();
//
// Set state to the keyframe
- last_ptr_->getFramePtr()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+ last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
// Establish factors
establishFactors();
// Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(last_ptr_->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
+ getProblem()->keyFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
// Update pointers
resetDerived();
@@ -203,9 +203,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We do not create a KF
// Advance this
- last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+ last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
last_ptr_->remove();
- incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
+ incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
// Update pointers
advanceDerived();
@@ -257,7 +257,7 @@ void ProcessorTracker::computeProcessingStep()
if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
{
- if (last_ptr_->getFramePtr()->isKey())
+ if (last_ptr_->getFrame()->isKey())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 6d9425664..fdeacd6ca 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -154,10 +154,10 @@ void ProcessorTrackerFeature::establishFactors()
if (ctr_ptr != nullptr) // factor links
{
- FrameBasePtr frm = ctr_ptr->getFrameOtherPtr();
+ FrameBasePtr frm = ctr_ptr->getFrameOther();
if (frm)
frm->addConstrainedBy(ctr_ptr);
- CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr();
+ CaptureBasePtr cap = ctr_ptr->getCaptureOther();
if (cap)
cap->addConstrainedBy(ctr_ptr);
}
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 3b7132ad5..4414d8caf 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -46,11 +46,11 @@ void ProcessorTrackerFeatureCorner::preProcess()
R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensorPtr()->extrinsicsInCaptures() || !getSensorPtr()->getPPtr()->isFixed()
- || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+ if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
+ || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
{
- t_robot_sensor_.head<2>() = getSensorPtr()->getPPtr()->getState();
- t_robot_sensor_(2) = getSensorPtr()->getOPtr()->getState()(0);
+ t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
+ t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
extrinsics_transformation_computed_ = true;
}
@@ -120,8 +120,8 @@ FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _featur
// Getting landmark ptr
LandmarkCorner2DPtr landmark_ptr = nullptr;
for (auto factor : _feature_other_ptr->getFactorList())
- if (factor->getLandmarkOtherPtr() != nullptr && factor->getLandmarkOtherPtr()->getType() == "CORNER 2D")
- landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(factor->getLandmarkOtherPtr());
+ if (factor->getLandmarkOther() != nullptr && factor->getLandmarkOther()->getType() == "CORNER 2D")
+ landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(factor->getLandmarkOther());
if (landmark_ptr == nullptr)
{
@@ -149,7 +149,7 @@ void ProcessorTrackerFeatureCorner::extractCorners(CaptureLaser2DPtr _capture_la
std::cout << "Extracting corners..." << std::endl;
corner_finder_.findCorners(_capture_laser_ptr->getScan(),
- (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(),
+ (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(),
line_finder_,
corners);
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index b5e95ad66..126a504e9 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -312,7 +312,7 @@ void ProcessorTrackerFeatureTrifocal::resetDerived()
void ProcessorTrackerFeatureTrifocal::preProcess()
{
- WOLF_TRACE("-------- Image ", getIncomingPtr()->id(), " -- t = ", getIncomingPtr()->getTimeStamp(), " s ----------");
+ WOLF_TRACE("-------- Image ", getIncoming()->id(), " -- t = ", getIncoming()->getTimeStamp(), " s ----------");
if (!initialized_)
{
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 65e061e46..9c67c53ad 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -117,7 +117,7 @@ unsigned int ProcessorTrackerLandmark::processKnown()
{
// Find landmarks in incoming_ptr_
FeatureBasePtrList known_features_list_incoming;
- unsigned int n = findLandmarks(getProblem()->getMapPtr()->getLandmarkList(),
+ unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
known_features_list_incoming, matches_landmark_from_incoming_);
// Append found incoming features
incoming_ptr_->addFeatureList(known_features_list_incoming);
@@ -137,10 +137,10 @@ void ProcessorTrackerLandmark::establishFactors()
{
last_feature->addFactor(ctr_ptr);
lmk->addConstrainedBy(ctr_ptr);
- FrameBasePtr frm = ctr_ptr->getFrameOtherPtr();
+ FrameBasePtr frm = ctr_ptr->getFrameOther();
if (frm)
frm->addConstrainedBy(ctr_ptr);
- CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr();
+ CaptureBasePtr cap = ctr_ptr->getCaptureOther();
if (cap)
cap->addConstrainedBy(ctr_ptr);
}
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index 147ac55e4..aeba30f8e 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -17,11 +17,11 @@ void ProcessorTrackerLandmarkCorner::preProcess()
R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensorPtr()->extrinsicsInCaptures() || !getSensorPtr()->getPPtr()->isFixed()
- || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+ if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
+ || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
{
- t_robot_sensor_.head<2>() = getSensorPtr()->getPPtr()->getState();
- t_robot_sensor_(2) = getSensorPtr()->getOPtr()->getState()(0);
+ t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
+ t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
extrinsics_transformation_computed_ = true;
}
@@ -206,7 +206,7 @@ bool ProcessorTrackerLandmarkCorner::voteForKeyFrame()
// option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
for (auto new_feat : new_features_last_)
{
- if (last_ptr_->getFramePtr()->id() - matches_landmark_from_last_[new_feat]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getFramePtr()->id() > loop_frames_th_)
+ if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_feat]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > loop_frames_th_)
{
std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
return true;
@@ -228,7 +228,7 @@ void ProcessorTrackerLandmarkCorner::extractCorners(CaptureLaser2DPtr _capture_l
// TODO: sort corners by bearing
std::list<laserscanutils::CornerPoint> corners;
- corner_finder_.findCorners(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), line_finder_, corners);
+ corner_finder_.findCorners(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), line_finder_, corners);
Eigen::Vector4s measurement;
for (auto corner : corners)
@@ -282,38 +282,38 @@ void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBasePtr _landmark_p
Eigen::Matrix3s& expected_feature_cov_)
{
//std::cout << "ProcessorTrackerLandmarkCorner::expectedFeature: " << std::endl;
- //std::cout << "Landmark global pose: " << _landmark_ptr->getPPtr()->getVector().transpose() << " " << _landmark_ptr->getOPtr()->getVector() << std::endl;
+ //std::cout << "Landmark global pose: " << _landmark_ptr->getP()->getVector().transpose() << " " << _landmark_ptr->getO()->getVector() << std::endl;
//std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
//std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
// ------------ expected feature measurement
- expected_feature_.head<2>() = R_sensor_world_.topLeftCorner<2,2>() * (_landmark_ptr->getPPtr()->getState() - t_world_sensor_.head<2>());
- expected_feature_(2) = _landmark_ptr->getOPtr()->getState()(0) - t_world_sensor_(2);
+ expected_feature_.head<2>() = R_sensor_world_.topLeftCorner<2,2>() * (_landmark_ptr->getP()->getState() - t_world_sensor_.head<2>());
+ expected_feature_(2) = _landmark_ptr->getO()->getState()(0) - t_world_sensor_(2);
expected_feature_(3) = (std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr))->getAperture();
//std::cout << "Expected feature pose: " << expected_feature_.head<3>().transpose() << std::endl;
// ------------ expected feature covariance
Eigen::MatrixXs Sigma = Eigen::MatrixXs::Zero(6, 6);
// closer keyframe with computed covariance
- FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFramePtr() : nullptr);
+ FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFrame() : nullptr);
// If all covariance blocks are stored wolfproblem (filling upper diagonal only)
if (key_frame_ptr != nullptr &&
// Sigma_rr
- getProblem()->getCovarianceBlock(key_frame_ptr->getPPtr(), key_frame_ptr->getPPtr(), Sigma, 3, 3) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getPPtr(), key_frame_ptr->getOPtr(), Sigma, 3, 5) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getOPtr(), key_frame_ptr->getOPtr(), Sigma, 5, 5) &&
+ getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getP(), Sigma, 3, 3) &&
+ getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getO(), Sigma, 3, 5) &&
+ getProblem()->getCovarianceBlock(key_frame_ptr->getO(), key_frame_ptr->getO(), Sigma, 5, 5) &&
// Sigma_ll
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getPPtr(), Sigma, 0, 0) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getOPtr(), Sigma, 0, 2) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getOPtr(), _landmark_ptr->getOPtr(), Sigma, 2, 2) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getP(), Sigma, 0, 0) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getO(), Sigma, 0, 2) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getO(), _landmark_ptr->getO(), Sigma, 2, 2) &&
// Sigma_lr
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), key_frame_ptr->getPPtr(), Sigma, 0, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), key_frame_ptr->getOPtr(), Sigma, 0, 5) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getOPtr(), key_frame_ptr->getPPtr(), Sigma, 2, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getOPtr(), key_frame_ptr->getOPtr(), Sigma, 2, 5) )
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getP(), Sigma, 0, 3) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getO(), Sigma, 0, 5) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getP(), Sigma, 2, 3) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getO(), Sigma, 2, 5) )
{
// Jacobian
- Eigen::Vector2s p_robot_landmark = t_world_robot_.head<2>() - _landmark_ptr->getPPtr()->getState();
+ Eigen::Vector2s p_robot_landmark = t_world_robot_.head<2>() - _landmark_ptr->getP()->getState();
Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
Jlr.block<3, 3>(0, 3) = -R_world_sensor_.transpose();
Jlr.block<3, 3>(0, 3) = R_world_sensor_.transpose();
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index a9f56b9fa..6122b9088 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -159,13 +159,13 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrL
// project landmark into incoming capture
LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_in_ptr);
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensorPtr());
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensor());
Eigen::Vector4s point3D_hmg;
Eigen::Vector2s pixel;
landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
- pixel = pinhole::projectPoint(camera->getIntrinsicPtr()->getState(),
+ pixel = pinhole::projectPoint(camera->getIntrinsic()->getState(),
camera->getDistortionVector(),
point3D_hmg.head<3>());
@@ -283,7 +283,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
{
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
- FrameBasePtr anchor_frame = getLastPtr()->getFramePtr();
+ FrameBasePtr anchor_frame = getLast()->getFrame();
Eigen::Vector2s point2D;
point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
@@ -292,8 +292,8 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
Scalar distance = params_tracker_landmark_image_->distance; // arbitrary value
Eigen::Vector4s vec_homogeneous;
- point2D = pinhole::depixellizePoint(getSensorPtr()->getIntrinsicPtr()->getState(),point2D);
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensorPtr()))->getCorrectionVector(),point2D);
+ point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
+ point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
Eigen::Vector3s point3D;
point3D.head<2>() = point2D;
@@ -303,7 +303,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
- LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensorPtr(), feat_point_image_ptr->getDescriptor());
+ LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensor(), feat_point_image_ptr->getDescriptor());
_feature_ptr->setLandmarkId(lmk_ahp_ptr->id());
return lmk_ahp_ptr;
}
@@ -311,9 +311,9 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
FactorBasePtr ProcessorTrackerLandmarkImage::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
- if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFramePtr())
+ if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFrame())
{
- return FactorBasePtr();
+ return FactorBase();
}
else
{
@@ -365,8 +365,8 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
Transform<Scalar,3,Eigen::Affine> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
// world to anchor robot frame
- Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getPPtr()->getState()); // sadly we cannot put a Map over a translation
- const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getOPtr()->getState()));
+ Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getP()->getState()); // sadly we cannot put a Map over a translation
+ const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getO()->getState()));
T_W_R0 = t_w_r0 * q_w_r0;
// world to current robot frame
@@ -375,17 +375,17 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
T_W_R1 = t_w_r1 * q_w_r1;
// anchor robot to anchor camera
- Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getPPtr()->getState());
- const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getOPtr()->getState()));
+ Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getP()->getState());
+ const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getO()->getState()));
T_R0_C0 = t_r0_c0 * q_r0_c0;
// current robot to current camera
- Translation<Scalar,3> t_r1_c1(this->getSensorPtr()->getPPtr()->getState());
- const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensorPtr()->getOPtr()->getState()));
+ Translation<Scalar,3> t_r1_c1(this->getSensor()->getP()->getState());
+ const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensor()->getO()->getState()));
T_R1_C1 = t_r1_c1 * q_r1_c1;
// Transform lmk from c0 to c1 and exit
- Vector4s landmark_hmg_c0 = _landmark->getPPtr()->getState(); // lmk in anchor frame
+ Vector4s landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
_point3D_hmg = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
}
@@ -447,17 +447,17 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
{
unsigned int num_lmks_in_img = 0;
- Eigen::VectorXs current_state = last_ptr_->getFramePtr()->getState();
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensorPtr());
+ Eigen::VectorXs current_state = last_ptr_->getFrame()->getState();
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
- for (auto landmark_base_ptr : getProblem()->getMapPtr()->getLandmarkList())
+ for (auto landmark_base_ptr : getProblem()->getMap()->getLandmarkList())
{
LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_base_ptr);
Eigen::Vector4s point3D_hmg;
landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
- Eigen::Vector2s point2D = pinhole::projectPoint(camera->getIntrinsicPtr()->getState(), // k
+ Eigen::Vector2s point2D = pinhole::projectPoint(camera->getIntrinsic()->getState(), // k
camera->getDistortionVector(), // d
point3D_hmg.head(3)); // v
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
index d33eb4496..a8037d037 100644
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ b/src/processor/processor_tracker_landmark_polyline.cpp
@@ -25,11 +25,11 @@ void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _
Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensorPtr()->extrinsicsInCaptures() || !getSensorPtr()->getPPtr()->isFixed()
- || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+ if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
+ || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
{
- t_robot_sensor_ = getSensorPtr()->getPPtr()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensorPtr()->getOPtr()->getState()(0)).matrix();
+ t_robot_sensor_ = getSensor()->getP()->getState();
+ R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
extrinsics_transformation_computed_ = true;
}
@@ -304,7 +304,7 @@ bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
// option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
for (auto new_ftr : new_features_last_)
{
- if (last_ptr_->getFramePtr()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getFramePtr()->id() > params_->loop_frames_th)
+ if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th)
{
std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
return true;
@@ -320,7 +320,7 @@ void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _captu
// TODO: sort corners by bearing
std::list<laserscanutils::Polyline> polylines;
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines);
+ line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
for (auto&& pl : polylines)
{
@@ -368,8 +368,8 @@ void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark
////////// landmark with origin
else
{
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getOPtr()->getState()(0)).matrix();
- const Eigen::VectorXs& t_world_points = polyline_landmark->getPPtr()->getState();
+ Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix();
+ const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState();
for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
{
@@ -951,20 +951,20 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _l
polyline_ptr->classify(object_class[classification]);
// Unfix origin
- polyline_ptr->getPPtr()->unfix();
- polyline_ptr->getOPtr()->unfix();
+ polyline_ptr->getP()->unfix();
+ polyline_ptr->getO()->unfix();
// Move origin to B
- polyline_ptr->getPPtr()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
+ polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id));
- polyline_ptr->getOPtr()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
+ polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
//std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl;
//std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl;
//std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl;
//std::cout << "frame_x: " << frame_x.transpose() << std::endl;
- //std::cout << "frame position: " << polyline_ptr->getPPtr()->getVector().transpose() << std::endl;
- //std::cout << "frame orientation: " << polyline_ptr->getOPtr()->getVector() << std::endl;
+ //std::cout << "frame position: " << polyline_ptr->getP()->getVector().transpose() << std::endl;
+ //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl;
// Fix polyline points to its respective relative positions
if (configuration)
@@ -994,7 +994,7 @@ void ProcessorTrackerLandmarkPolyline::postProcess()
//std::cout << "postProcess: " << std::endl;
//std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl;
//std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl;
- classifyPolilines(getProblem()->getMapPtr()->getLandmarkList());
+ classifyPolilines(getProblem()->getMap()->getLandmarkList());
}
FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
index 7745e65b1..323616be9 100644
--- a/src/sensor/sensor_GPS.cpp
+++ b/src/sensor/sensor_GPS.cpp
@@ -24,12 +24,12 @@ SensorGPS::~SensorGPS()
//
}
-StateBlockPtr SensorGPS::getMapPPtr() const
+StateBlockPtr SensorGPS::getMapP() const
{
return getStateBlockPtrStatic(3);
}
-StateBlockPtr SensorGPS::getMapOPtr() const
+StateBlockPtr SensorGPS::getMapO() const
{
return getStateBlockPtrStatic(4);
}
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index 427dc4518..c69f99b6a 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/sensor_GPS_fix.cpp
@@ -29,7 +29,7 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
&& "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
- sen->getPPtr()->fix();
+ sen->getP()->fix();
sen->setName(_unique_name);
return sen;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index cf16db04b..dd821901c 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -222,7 +222,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void)
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
FrameBaseRevIter frame_rev_it = frame_list.rbegin();
while (frame_rev_it != frame_list.rend())
{
@@ -242,7 +242,7 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
{
// we search for the most recent Capture of this sensor before _ts
CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
FrameBaseRevIter frame_rev_it = frame_list.rbegin();
@@ -275,17 +275,17 @@ StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
return getStateBlockPtr(2, _ts);
}
-StateBlockPtr SensorBase::getPPtr()
+StateBlockPtr SensorBase::getP()
{
return getStateBlockPtr(0);
}
-StateBlockPtr SensorBase::getOPtr()
+StateBlockPtr SensorBase::getO()
{
return getStateBlockPtr(1);
}
-StateBlockPtr SensorBase::getIntrinsicPtr()
+StateBlockPtr SensorBase::getIntrinsic()
{
return getStateBlockPtr(2);
}
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 6e20ff960..54c08c69f 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -19,7 +19,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsC
{
assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
useRawImages();
- pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
+ pinhole::computeCorrectionModel(getIntrinsic()->getState(), distortion_, correction_);
}
SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) :
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index d710d9e77..d5fdb9e5e 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -62,7 +62,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
// return intrinsics_;
//}
-//void SensorDiffDrive::initIntrisicsPtr()
+//void SensorDiffDrive::initIntrisics()
//{
// assert(intrinsic_ptr_ == nullptr &&
// "SensorDiffDrive::initIntrisicsPtr should only be called once at construction.");
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 148bfb61a..5fe532174 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -110,7 +110,7 @@ void SolverManager::update()
assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}
-wolf::ProblemPtr SolverManager::getProblemPtr()
+wolf::ProblemPtr SolverManager::getProblem()
{
return wolf_problem_;
}
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index e21db97fe..7d3081ad7 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -49,7 +49,7 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
// ADD CONSTRAINTS
FactorBasePtrList ctr_list;
- _problem_ptr->getTrajectoryPtr()->getFactorList(ctr_list);
+ _problem_ptr->getTrajectory()->getFactorList(ctr_list);
//std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
@@ -85,31 +85,31 @@ void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
{
// TODO
//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
+ //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
break;
}
case ST_THETA:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_1D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_VECTOR:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
@@ -136,9 +136,9 @@ void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
// TODO
// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
+// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
+// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
// else
// printf("\nERROR: Update state unit status with unknown status");
//
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
index 8bb0e0abe..54c2bd50b 100644
--- a/src/track_matrix.cpp
+++ b/src/track_matrix.cpp
@@ -49,7 +49,7 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr
assert( (_track_id > 0) && (_track_id <= track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
_ftr->setTrackId(_track_id);
- if (_cap != _ftr->getCapturePtr())
+ if (_cap != _ftr->getCapture())
_ftr->setCapturePtr(_cap);
tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
snapshots_[_cap->id()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
@@ -62,7 +62,7 @@ void TrackMatrix::remove(size_t _track_id)
{
for (auto const& pair_time_ftr : tracks_.at(_track_id))
{
- SizeStd cap_id = pair_time_ftr.second->getCapturePtr()->id();
+ SizeStd cap_id = pair_time_ftr.second->getCapture()->id();
snapshots_.at(cap_id).erase(_track_id);
if (snapshots_.at(cap_id).empty())
snapshots_.erase(cap_id);
@@ -94,10 +94,10 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
void TrackMatrix::remove(FeatureBasePtr _ftr)
{
- // assumes _ftr->getCapturePtr() and _ftr->trackId() are valid
+ // assumes _ftr->getCapture() and _ftr->trackId() are valid
if (_ftr)
{
- if (auto cap = _ftr->getCapturePtr())
+ if (auto cap = _ftr->getCapture())
{
tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
if (tracks_.at(_ftr->trackId()).empty())
@@ -191,7 +191,7 @@ FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
{
- return firstFeature(_track_id)->getCapturePtr();
+ return firstFeature(_track_id)->getCapture();
}
}
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index a82713667..2e96c6a72 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -47,7 +47,7 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _ctr_list)
void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
{
moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
- // last_key_frame_ptr_ = findLastKeyFramePtr(); // done in moveFrame() just above
+ // last_key_frame_ptr_ = findLastKeyFrame(); // done in moveFrame() just above
}
void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
@@ -56,7 +56,7 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
{
frame_list_.remove(_frm_ptr);
frame_list_.insert(_place, _frm_ptr);
- last_key_frame_ptr_ = findLastKeyFramePtr();
+ last_key_frame_ptr_ = findLastKeyFrame();
}
}
@@ -68,7 +68,7 @@ FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
return getFrameList().begin();
}
-FrameBasePtr TrajectoryBase::findLastKeyFramePtr()
+FrameBasePtr TrajectoryBase::findLastKeyFrame()
{
// NOTE: Assumes keyframes are sorted by timestamp
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index 5748e8a57..7487eb2fe 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -276,8 +276,8 @@ class Process_Factor_IMU : public testing::Test
sensor_imu->process(capture_imu);
- D_preint = processor_imu->getLastPtr()->getDeltaPreint();
- D_corrected = processor_imu->getLastPtr()->getDeltaCorrected(bias_real);
+ D_preint = processor_imu->getLast()->getDeltaPreint();
+ D_corrected = processor_imu->getLast()->getDeltaCorrected(bias_real);
}
return processor_imu->getBuffer();
}
@@ -311,9 +311,9 @@ class Process_Factor_IMU : public testing::Test
// wolf objects
KF_0 = problem->setPrior(x0, P0, t0, dt/2);
- C_0 = processor_imu->getOriginPtr();
+ C_0 = processor_imu->getOrigin();
- processor_imu->getLastPtr()->setCalibrationPreint(bias_preint);
+ processor_imu->getLast()->setCalibrationPreint(bias_preint);
return true;
}
@@ -441,12 +441,12 @@ class Process_Factor_IMU : public testing::Test
void setFixedBlocks()
{
// this sets each state block status fixed / unfixed
- KF_0->getPPtr()->setFixed(p0_fixed);
- KF_0->getOPtr()->setFixed(q0_fixed);
- KF_0->getVPtr()->setFixed(v0_fixed);
- KF_1->getPPtr()->setFixed(p1_fixed);
- KF_1->getOPtr()->setFixed(q1_fixed);
- KF_1->getVPtr()->setFixed(v1_fixed);
+ KF_0->getP()->setFixed(p0_fixed);
+ KF_0->getO()->setFixed(q0_fixed);
+ KF_0->getV()->setFixed(v0_fixed);
+ KF_1->getP()->setFixed(p1_fixed);
+ KF_1->getO()->setFixed(q1_fixed);
+ KF_1->getV()->setFixed(v1_fixed);
}
void setKfStates()
@@ -490,7 +490,7 @@ class Process_Factor_IMU : public testing::Test
capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
processor_imu->process(capture_imu);
- KF_1 = problem->getLastKeyFramePtr();
+ KF_1 = problem->getLastKeyFrame();
C_1 = KF_1->getCaptureList().front(); // front is IMU
CM_1 = static_pointer_cast<CaptureMotion>(C_1);
@@ -747,7 +747,7 @@ TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
// ===================================== RUN ALL
Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished());
- sensor_imu->getIntrinsicPtr()->setState(initial_bias);
+ sensor_imu->getIntrinsic()->setState(initial_bias);
configureAll();
integrateAll();
buildProblem();
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index d1ec1884c..b6f183675 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -18,9 +18,9 @@ TEST(CaptureBase, ConstructorNoSensor)
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
ASSERT_EQ(C->getTimeStamp(), 1.2);
- ASSERT_FALSE(C->getFramePtr());
+ ASSERT_FALSE(C->getFrame());
ASSERT_FALSE(C->getProblem());
- ASSERT_FALSE(C->getSensorPtr());
+ ASSERT_FALSE(C->getSensor());
}
TEST(CaptureBase, ConstructorWithSensor)
@@ -28,11 +28,11 @@ TEST(CaptureBase, ConstructorWithSensor)
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
ASSERT_EQ(C->getTimeStamp(), 1.5);
- ASSERT_FALSE(C->getFramePtr());
+ ASSERT_FALSE(C->getFrame());
ASSERT_FALSE(C->getProblem());
- ASSERT_TRUE(C->getSensorPtr());
- ASSERT_FALSE(C->getSensorPPtr());
- ASSERT_FALSE(C->getSensorOPtr());
+ ASSERT_TRUE(C->getSensor());
+ ASSERT_FALSE(C->getSensorP());
+ ASSERT_FALSE(C->getSensorO());
}
TEST(CaptureBase, Static_sensor_params)
@@ -44,14 +44,14 @@ TEST(CaptureBase, Static_sensor_params)
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
// query sensor blocks
- ASSERT_EQ(S->getPPtr(), p);
- ASSERT_EQ(S->getOPtr(), o);
- ASSERT_EQ(S->getIntrinsicPtr(), i);
+ ASSERT_EQ(S->getP(), p);
+ ASSERT_EQ(S->getO(), o);
+ ASSERT_EQ(S->getIntrinsic(), i);
// query capture blocks
- ASSERT_EQ(C->getSensorPPtr(), p);
- ASSERT_EQ(C->getSensorOPtr(), o);
- ASSERT_EQ(C->getSensorIntrinsicPtr(), i);
+ ASSERT_EQ(C->getSensorP(), p);
+ ASSERT_EQ(C->getSensorO(), o);
+ ASSERT_EQ(C->getSensorIntrinsic(), i);
}
TEST(CaptureBase, Dynamic_sensor_params)
@@ -63,14 +63,14 @@ TEST(CaptureBase, Dynamic_sensor_params)
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
- // ASSERT_EQ(S->getPPtr(), p);
- // ASSERT_EQ(S->getOPtr(), o);
- // ASSERT_EQ(S->getIntrinsicPtr(), i);
+ // ASSERT_EQ(S->getP(), p);
+ // ASSERT_EQ(S->getO(), o);
+ // ASSERT_EQ(S->getIntrinsic(), i);
// query capture blocks
- ASSERT_EQ(C->getSensorPPtr(), p);
- ASSERT_EQ(C->getSensorOPtr(), o);
- ASSERT_EQ(C->getSensorIntrinsicPtr(), i);
+ ASSERT_EQ(C->getSensorP(), p);
+ ASSERT_EQ(C->getSensorO(), o);
+ ASSERT_EQ(C->getSensorIntrinsic(), i);
}
TEST(CaptureBase, addFeature)
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 98d0e9256..d1a593c0e 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -68,7 +68,7 @@ class CeresManagerWrapper : public CeresManager
bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
{
return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrizationPtr() == local_param &&
+ state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
};
@@ -85,7 +85,7 @@ TEST(CeresManager, Create)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// check double ointers to branches
- ASSERT_EQ(P, ceres_manager_ptr->getProblemPtr());
+ ASSERT_EQ(P, ceres_manager_ptr->getProblem());
// run ceres manager check
ceres_manager_ptr->check();
@@ -552,15 +552,15 @@ TEST(CeresManager, FactorsRemoveLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getOPtr())));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getOPtr())));
+ FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+ FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),F->getOPtr()->getLocalParametrizationPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
// check factor
ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
@@ -568,13 +568,13 @@ TEST(CeresManager, FactorsRemoveLocalParam)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// remove local param
- F->getOPtr()->removeLocalParametrization();
+ F->getO()->removeLocalParametrization();
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
+ ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
// check factor
ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
@@ -594,15 +594,15 @@ TEST(CeresManager, FactorsUpdateLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getOPtr())));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getOPtr())));
+ FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+ FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),F->getOPtr()->getLocalParametrizationPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
// check factor
ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
@@ -611,14 +611,14 @@ TEST(CeresManager, FactorsUpdateLocalParam)
// remove local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getOPtr()->setLocalParametrizationPtr(local_param_ptr);
+ F->getO()->setLocalParametrizationPtr(local_param_ptr);
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),local_param_ptr));
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr));
// check factor
ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index 5afee4d90..b7d3143fa 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -108,7 +108,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
}
- KF1 = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -192,7 +192,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
sen_imu->process(imu_ptr);
}
- KF1 = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -276,7 +276,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -362,7 +362,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -449,7 +449,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -532,7 +532,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -625,7 +625,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -724,7 +724,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -823,7 +823,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
}
expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -930,8 +930,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity());
Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity());
- WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+ WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
+ WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
for(unsigned int i = 1; i<=1000; i++)
{
@@ -955,16 +955,16 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
capture_imu->setData(data_imu);
sensor_imu->process(capture_imu);
- WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+ WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
+ WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
//when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
if(t_imu.get() >= t_odo.get())
{
WOLF_TRACE("====== create ODOM KF ========");
-// WOLF_TRACE("Jac calib: ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLastPtr()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLastPtr()->getCalibrationPreint().transpose());
+// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
+// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
+// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
// PROCESS ODOM 3D DATA
data_odo.head(3) << 0,0,0;
@@ -972,15 +972,15 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
capture_odo->setTimeStamp(t_odo);
capture_odo->setData(data_odo);
-// WOLF_TRACE("Jac calib: ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLastPtr()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLastPtr()->getCalibrationPreint().transpose());
+// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
+// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
+// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
sensor_odo->process(capture_odo);
-// WOLF_TRACE("Jac calib: ", processor_imu->getOriginPtr()->getJacobianCalib().row(0));
-// WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", processor_imu->getOriginPtr()->getCalibrationPreint().transpose());
+// WOLF_TRACE("Jac calib: ", processor_imu->getOrigin()->getJacobianCalib().row(0));
+// WOLF_TRACE("orig calib: ", processor_imu->getOrigin()->getCalibration().transpose());
+// WOLF_TRACE("orig calib preint: ", processor_imu->getOrigin()->getCalibrationPreint().transpose());
//prepare next odometry measurement
quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
@@ -992,7 +992,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
expected_final_state.resize(10);
expected_final_state << 0,0,0, quat_imu.x(), quat_imu.y(), quat_imu.z(), quat_imu.w(), 0,0,0;
- last_KF = problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t_imu);
+ last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t_imu);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -1125,7 +1125,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
imu_ptr->setData(data_imu);
sen_imu->process(imu_ptr);
- D_cor = processor_ptr_imu->getLastPtr()->getDeltaCorrected(origin_bias);
+ D_cor = processor_ptr_imu->getLast()->getDeltaCorrected(origin_bias);
if(ts.get() >= t_odom.get())
{
@@ -1145,7 +1145,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
}
expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose());
@@ -1321,7 +1321,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
}
expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -1338,18 +1338,18 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
KF0->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<1,2,3,1,2,3).finished());
- KF1->getPPtr()->setState(expected_final_state.head(3));
- KF1->getOPtr()->setState(expected_final_state.segment(3,4));
- KF1->getVPtr()->setState(expected_final_state.segment(7,3));
+ KF1->getP()->setState(expected_final_state.head(3));
+ KF1->getO()->setState(expected_final_state.segment(3,4));
+ KF1->getV()->setState(expected_final_state.segment(7,3));
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished());
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1365,12 +1365,12 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1456,17 +1456,17 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
- KF1->getPPtr()->setState(expected_final_state.head(3));
- KF1->getOPtr()->setState(expected_final_state.segment(3,4));
- KF1->getVPtr()->setState(expected_final_state.segment(7,3));
+ KF1->getP()->setState(expected_final_state.head(3));
+ KF1->getO()->setState(expected_final_state.segment(3,4));
+ KF1->getV()->setState(expected_final_state.segment(7,3));
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1481,12 +1481,12 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK
TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1572,17 +1572,17 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
- last_KF->getPPtr()->setState(expected_final_state.head(3));
- last_KF->getOPtr()->setState(expected_final_state.segment(3,4));
- last_KF->getVPtr()->setState(expected_final_state.segment(7,3));
+ last_KF->getP()->setState(expected_final_state.head(3));
+ last_KF->getO()->setState(expected_final_state.segment(3,4));
+ last_KF->getV()->setState(expected_final_state.segment(7,3));
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1594,12 +1594,12 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO
TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1674,12 +1674,12 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1755,15 +1755,15 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
@@ -1776,12 +1776,12 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1854,15 +1854,15 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
@@ -1875,12 +1875,12 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1954,15 +1954,15 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -1975,12 +1975,12 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO
TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -2054,15 +2054,15 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2075,12 +2075,12 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -2154,13 +2154,13 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
last_KF->setState(expected_final_state);
@@ -2175,13 +2175,13 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
//TEST_F(FactorIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
//{
// //prepare problem for solving
-// origin_KF->getPPtr()->fix();
-// origin_KF->getOPtr()->fix();
-// origin_KF->getVPtr()->unfix();
+// origin_KF->getP()->fix();
+// origin_KF->getO()->fix();
+// origin_KF->getV()->unfix();
//
-// last_KF->getPPtr()->unfix();
-// last_KF->getOPtr()->fix();
-// last_KF->getVPtr()->unfix();
+// last_KF->getP()->unfix();
+// last_KF->getO()->fix();
+// last_KF->getV()->unfix();
//
// last_KF->setState(expected_final_state);
//
@@ -2196,15 +2196,15 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2241,15 +2241,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2262,7 +2262,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
@@ -2288,15 +2288,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2308,24 +2308,24 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2336,10 +2336,10 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
@@ -2347,15 +2347,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2364,26 +2364,26 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
@@ -2392,27 +2392,27 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2422,13 +2422,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2462,15 +2462,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
FactorFix3DPtr ctr_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->unfix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->unfix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2480,13 +2480,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2520,15 +2520,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
FactorFix3DPtr ctr_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->unfix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->unfix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2538,13 +2538,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2570,15 +2570,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2621,15 +2621,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2642,7 +2642,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
@@ -2667,15 +2667,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2684,25 +2684,25 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_in
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2711,27 +2711,27 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2740,25 +2740,25 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_in
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
@@ -2767,27 +2767,27 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2797,13 +2797,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index d6d7c4b84..64d71f268 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -49,7 +49,7 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS"
TEST(FactorBlockAbs, ctr_block_abs_p)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
- fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFramePtr()->getPPtr()));
+ fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
ASSERT_TRUE(problem_ptr->check(0));
@@ -61,13 +61,13 @@ TEST(FactorBlockAbs, ctr_block_abs_p)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState(), pose10.head<3>(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
}
TEST(FactorBlockAbs, ctr_block_abs_p_tail2)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
- fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFramePtr()->getPPtr(),1,2));
+ fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -77,13 +77,13 @@ TEST(FactorBlockAbs, ctr_block_abs_p_tail2)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
}
TEST(FactorBlockAbs, ctr_block_abs_v)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
- fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFramePtr()->getVPtr()));
+ fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
ASSERT_TRUE(problem_ptr->check(0));
@@ -95,13 +95,13 @@ TEST(FactorBlockAbs, ctr_block_abs_v)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getVPtr()->getState(), pose10.tail<3>(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
}
TEST(FactorQuatAbs, ctr_block_abs_o)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
- fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()));
+ fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
ASSERT_TRUE(problem_ptr->check(0));
@@ -113,7 +113,7 @@ TEST(FactorQuatAbs, ctr_block_abs_o)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getOPtr()->getState(), pose10.segment<4>(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 8883377ef..037222713 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -41,11 +41,11 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
feature_ptr->addFactor(factor_ptr);
fr1_ptr->addConstrainedBy(factor_ptr);
- ASSERT_TRUE(factor_ptr->getFeaturePtr());
- ASSERT_TRUE(factor_ptr->getFeaturePtr()->getCapturePtr());
- ASSERT_TRUE(factor_ptr->getFeaturePtr()->getCapturePtr()->getFramePtr());
- ASSERT_TRUE(factor_ptr->getFeaturePtr()->getCapturePtr()->getSensorPtr());
- ASSERT_TRUE(factor_ptr->getFrameOtherPtr());
+ ASSERT_TRUE(factor_ptr->getFeature());
+ ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
+ ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getFrame());
+ ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getSensor());
+ ASSERT_TRUE(factor_ptr->getFrameOther());
}
TEST(CaptureAutodiff, ResidualOdom2d)
@@ -80,10 +80,10 @@ TEST(CaptureAutodiff, ResidualOdom2d)
// EVALUATE
- Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
- Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
- Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
- Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+ Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+ Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+ Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+ Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
@@ -127,10 +127,10 @@ TEST(CaptureAutodiff, JacobianOdom2d)
// COMPUTE JACOBIANS
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+ const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
@@ -212,10 +212,10 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
// COMPUTE JACOBIANS
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+ const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index f79fdcf25..1de85bd61 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -101,7 +101,7 @@ TEST_F(FactorAutodiffDistance3D_Test, solve)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
// Check distance between F1 and F2 positions -- must match the measurement
- ASSERT_NEAR( (F1->getPPtr()->getState() - F2->getPPtr()->getState()).norm(), measurement, 1e-10);
+ ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 18a487d78..1f872726f 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -331,20 +331,20 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F1)
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -362,8 +362,8 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F1)
pose_perturbated.segment(3,4).normalize();
F1->setState(pose_perturbated);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -383,14 +383,14 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F1)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -412,21 +412,21 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F2)
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -444,8 +444,8 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F2)
pose_perturbated.segment(3,4).normalize();
F2->setState(pose_perturbated);
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -465,14 +465,14 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F2)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -494,21 +494,21 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F3)
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -526,8 +526,8 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F3)
pose_perturbated.segment(3,4).normalize();
F3->setState(pose_perturbated);
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -548,14 +548,14 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F3)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -577,21 +577,21 @@ TEST_F(FactorAutodiffTrifocalTest, solve_S)
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -599,7 +599,7 @@ TEST_F(FactorAutodiffTrifocalTest, solve_S)
S_p. data(), S_o. data(),
res.data());
- WOLF_DEBUG("Initial state: ", S->getPPtr()->getState().transpose(), " ", S->getOPtr()->getState().transpose());
+ WOLF_DEBUG("Initial state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
WOLF_DEBUG("residual before perturbing: ", res.transpose());
ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
@@ -609,11 +609,11 @@ TEST_F(FactorAutodiffTrifocalTest, solve_S)
Vector4s ori_perturbated = vquat_cam + 0.1 * Vector4s::Random();
ori_perturbated.normalize();
Vector7s pose_perturbated; pose_perturbated << pos_perturbated, ori_perturbated;
- S->getPPtr()->setState(pos_perturbated);
- S->getOPtr()->setState(ori_perturbated);
+ S->getP()->setState(pos_perturbated);
+ S->getO()->setState(ori_perturbated);
- S_p = S->getPPtr()->getState();
- S_o = S->getOPtr()->getState();
+ S_p = S->getP()->getState();
+ S_o = S->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -633,14 +633,14 @@ TEST_F(FactorAutodiffTrifocalTest, solve_S)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -648,7 +648,7 @@ TEST_F(FactorAutodiffTrifocalTest, solve_S)
S_p. data(), S_o. data(),
res.data());
- WOLF_DEBUG("solved state: ", S->getPPtr()->getState().transpose(), " ", S->getOPtr()->getState().transpose());
+ WOLF_DEBUG("solved state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
WOLF_DEBUG("residual after solve: ", res.transpose());
WOLF_DEBUG(report, " AND UNION");
@@ -732,22 +732,22 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point)
*
*/
- S ->getPPtr()->fix(); // do not calibrate sensor pos
- S ->getOPtr()->fix(); // do not calibrate sensor ori
- F1->getPPtr()->fix(); // Cam 1 acts as reference
- F1->getOPtr()->fix(); // Cam 1 acts as reference
- F2->getPPtr()->fix(); // This fixes the scale
- F2->getOPtr()->unfix(); // Estimate Cam 2 ori
- F3->getPPtr()->unfix(); // Estimate Cam 3 pos
- F3->getOPtr()->unfix(); // Estimate Cam 3 ori
+ S ->getP()->fix(); // do not calibrate sensor pos
+ S ->getO()->fix(); // do not calibrate sensor ori
+ F1->getP()->fix(); // Cam 1 acts as reference
+ F1->getO()->fix(); // Cam 1 acts as reference
+ F2->getP()->fix(); // This fixes the scale
+ F2->getO()->unfix(); // Estimate Cam 2 ori
+ F3->getP()->unfix(); // Estimate Cam 3 pos
+ F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, keep scale
- F1->getPPtr()->setState( pos1 );
- F1->getOPtr()->setState( vquat1 );
- F2->getPPtr()->setState( pos2 ); // this fixes the scale
- F2->getOPtr()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getPPtr()->setState( pos3 + 0.2*Vector3s::Random());
- F3->getOPtr()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
+ F1->getP()->setState( pos1 );
+ F1->getO()->setState( vquat1 );
+ F2->getP()->setState( pos2 ); // this fixes the scale
+ F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
+ F3->getP()->setState( pos3 + 0.2*Vector3s::Random());
+ F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -756,19 +756,19 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getPPtr()->getState(), pos2 , 1e-10);
- ASSERT_MATRIX_APPROX(F2->getOPtr()->getState(), vquat2, 1e-10);
- ASSERT_MATRIX_APPROX(F3->getPPtr()->getState(), pos3 , 1e-10);
- ASSERT_MATRIX_APPROX(F3->getOPtr()->getState(), vquat3, 1e-10);
-
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-10);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-10);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-10);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-10);
+
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
// evaluate residuals
Vector3s res;
@@ -797,22 +797,22 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
*
*/
- S ->getPPtr()->fix(); // do not calibrate sensor pos
- S ->getOPtr()->fix(); // do not calibrate sensor ori
- F1->getPPtr()->fix(); // Cam 1 acts as reference
- F1->getOPtr()->fix(); // Cam 1 acts as reference
- F2->getPPtr()->fix(); // This fixes the scale
- F2->getOPtr()->unfix(); // Estimate Cam 2 ori
- F3->getPPtr()->unfix(); // Estimate Cam 3 pos
- F3->getOPtr()->unfix(); // Estimate Cam 3 ori
+ S ->getP()->fix(); // do not calibrate sensor pos
+ S ->getO()->fix(); // do not calibrate sensor ori
+ F1->getP()->fix(); // Cam 1 acts as reference
+ F1->getO()->fix(); // Cam 1 acts as reference
+ F2->getP()->fix(); // This fixes the scale
+ F2->getO()->unfix(); // Estimate Cam 2 ori
+ F3->getP()->unfix(); // Estimate Cam 3 pos
+ F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, change scale
- F1->getPPtr()->setState( 2 * pos1 );
- F1->getOPtr()->setState( vquat1 );
- F2->getPPtr()->setState( 2 * pos2 );
- F2->getOPtr()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getPPtr()->setState( 2 * pos3 + 0.2*Vector3s::Random());
- F3->getOPtr()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized());
+ F1->getP()->setState( 2 * pos1 );
+ F1->getO()->setState( vquat1 );
+ F2->getP()->setState( 2 * pos2 );
+ F2->getO()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized());
+ F3->getP()->setState( 2 * pos3 + 0.2*Vector3s::Random());
+ F3->getO()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized());
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -821,19 +821,19 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getPPtr()->getState(), 2 * pos2, 1e-8);
- ASSERT_MATRIX_APPROX(F2->getOPtr()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getPPtr()->getState(), 2 * pos3, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getOPtr()->getState(), vquat3, 1e-8);
-
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
// evaluate residuals
Vector3s res;
@@ -863,22 +863,22 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
*
*/
- S ->getPPtr()->fix(); // do not calibrate sensor pos
- S ->getOPtr()->fix(); // do not calibrate sensor ori
- F1->getPPtr()->fix(); // Cam 1 acts as reference
- F1->getOPtr()->fix(); // Cam 1 acts as reference
- F2->getPPtr()->unfix(); // Estimate Cam 2 pos
- F2->getOPtr()->unfix(); // Estimate Cam 2 ori
- F3->getPPtr()->unfix(); // Estimate Cam 3 pos
- F3->getOPtr()->unfix(); // Estimate Cam 3 ori
+ S ->getP()->fix(); // do not calibrate sensor pos
+ S ->getO()->fix(); // do not calibrate sensor ori
+ F1->getP()->fix(); // Cam 1 acts as reference
+ F1->getO()->fix(); // Cam 1 acts as reference
+ F2->getP()->unfix(); // Estimate Cam 2 pos
+ F2->getO()->unfix(); // Estimate Cam 2 ori
+ F3->getP()->unfix(); // Estimate Cam 3 pos
+ F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, change scale
- F1->getPPtr()->setState( pos1 );
- F1->getOPtr()->setState( vquat1 );
- F2->getPPtr()->setState( pos2 + 0.2*Vector3s::Random() );
- F2->getOPtr()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getPPtr()->setState( pos3 + 0.2*Vector3s::Random());
- F3->getOPtr()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
+ F1->getP()->setState( pos1 );
+ F1->getO()->setState( vquat1 );
+ F2->getP()->setState( pos2 + 0.2*Vector3s::Random() );
+ F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
+ F3->getP()->setState( pos3 + 0.2*Vector3s::Random());
+ F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
// Add a distance factor to fix the scale
Scalar distance = sqrt(2.0);
@@ -906,19 +906,19 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getPPtr()->getState(), pos2 , 1e-8);
- ASSERT_MATRIX_APPROX(F2->getOPtr()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getPPtr()->getState(), pos3 , 1e-8);
- ASSERT_MATRIX_APPROX(F3->getOPtr()->getState(), vquat3, 1e-8);
-
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-8);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
// evaluate residuals
Vector3s res;
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 2680c7f70..2e8245e15 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -75,16 +75,16 @@ class FeatureIMU_test : public testing::Test
sensor_ptr->process(imu_ptr);
//create Frame
- ts = problem->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = problem->getProcessorMotionPtr()->getCurrentState();
+ ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = problem->getProcessorMotion()->getCurrentState();
last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
- problem->getTrajectoryPtr()->addFrame(last_frame);
+ problem->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint = problem->getProcessorMotionPtr()->getMotion().delta_integr_;
- delta_preint_cov = problem->getProcessorMotionPtr()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
- VectorXs calib_preint = problem->getProcessorMotionPtr()->getBuffer().getCalibrationPreint();
- dD_db_jacobians = problem->getProcessorMotionPtr()->getMotion().jacobian_calib_;
+ delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
+ delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
+ VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint();
+ dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
feat_imu = std::make_shared<FeatureIMU>(delta_preint,
delta_preint_cov,
calib_preint,
@@ -113,7 +113,7 @@ TEST_F(FeatureIMU_test, check_frame)
// set variables
using namespace wolf;
- FrameBasePtr left_frame = feat_imu->getFramePtr();
+ FrameBasePtr left_frame = feat_imu->getFrame();
wolf::TimeStamp t;
left_frame->getTimeStamp(t);
origin_frame->getTimeStamp(ts);
@@ -124,12 +124,12 @@ TEST_F(FeatureIMU_test, check_frame)
ASSERT_TRUE(left_frame->isKey());
wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
- origin_pptr = origin_frame->getPPtr();
- origin_optr = origin_frame->getOPtr();
- origin_vptr = origin_frame->getVPtr();
- left_pptr = left_frame->getPPtr();
- left_optr = left_frame->getOPtr();
- left_vptr = left_frame->getVPtr();
+ origin_pptr = origin_frame->getP();
+ origin_optr = origin_frame->getO();
+ origin_vptr = origin_frame->getV();
+ left_pptr = left_frame->getP();
+ left_optr = left_frame->getO();
+ left_vptr = left_frame->getV();
ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
Eigen::Map<const Eigen::Quaternions> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index eb6a5417e..06436282a 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -39,16 +39,16 @@ TEST(FrameBase, StateBlocks)
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3);
- ASSERT_EQ(F->getPPtr()->getState().size(),(unsigned int) 2);
- ASSERT_EQ(F->getOPtr()->getState().size(),(unsigned int) 1);
- ASSERT_EQ(F->getVPtr(), nullptr);
+ ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
+ ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1);
+ ASSERT_EQ(F->getV(), nullptr);
}
TEST(FrameBase, LinksBasic)
{
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- ASSERT_FALSE(F->getTrajectoryPtr());
+ ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
// ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
// ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
@@ -63,12 +63,12 @@ TEST(FrameBase, LinksToTree)
{
// Problem with 2 frames and one motion factor between them
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
- P->getHardwarePtr()->addSensor(S);
+ P->getHardware()->addSensor(S);
FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
T->addFrame(F1);
FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
@@ -111,13 +111,13 @@ TEST(FrameBase, LinksToTree)
ASSERT_FALSE(F1->isFixed());
F1->fix();
ASSERT_TRUE(F1->isFixed());
- F1->getPPtr()->unfix();
+ F1->getP()->unfix();
ASSERT_FALSE(F1->isFixed());
F1->unfix();
ASSERT_FALSE(F1->isFixed());
- F1->getPPtr()->fix();
+ F1->getP()->fix();
ASSERT_FALSE(F1->isFixed());
- F1->getOPtr()->fix();
+ F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
// set key
@@ -151,9 +151,9 @@ TEST(FrameBase, GetSetState)
// Set the state, check that state blocks hold the current states
F.setState(x);
- ASSERT_TRUE((p - F.getPPtr()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((q - F.getOPtr()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((v - F.getVPtr()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// Get the state, form 1 by reference
F.getState(x1);
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 849d807cc..5f479b19e 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -73,7 +73,7 @@ void show(const ProblemPtr& problem)
using std::endl;
cout << std::setprecision(4);
- for (FrameBasePtr F : problem->getTrajectoryPtr()->getFrameList())
+ for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
{
@@ -412,7 +412,7 @@ TEST(Odom2D, KF_callback)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
@@ -453,7 +453,7 @@ TEST(Odom2D, KF_callback)
ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
- ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_2) , integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 6bb1eccbc..e39c87088 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -27,10 +27,10 @@ SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(proble
TEST(ParameterPrior, initial_extrinsics)
{
ASSERT_TRUE(problem_ptr->check(0));
- ASSERT_TRUE(odom_sensor_ptr_->getPPtr());
- ASSERT_TRUE(odom_sensor_ptr_->getOPtr());
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),initial_extrinsics.head(3),1e-9);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),initial_extrinsics.tail(4),1e-9);
+ ASSERT_TRUE(odom_sensor_ptr_->getP());
+ ASSERT_TRUE(odom_sensor_ptr_->getO());
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),initial_extrinsics.head(3),1e-9);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),initial_extrinsics.tail(4),1e-9);
}
TEST(ParameterPrior, prior_p)
@@ -39,7 +39,7 @@ TEST(ParameterPrior, prior_p)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior_extrinsics.head(3),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_o)
@@ -48,7 +48,7 @@ TEST(ParameterPrior, prior_o)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),prior_extrinsics.tail(4),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
}
TEST(ParameterPrior, prior_p_overwrite)
@@ -57,7 +57,7 @@ TEST(ParameterPrior, prior_p_overwrite)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior2_extrinsics.head(3),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_p_segment)
@@ -66,7 +66,7 @@ TEST(ParameterPrior, prior_p_segment)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
}
TEST(ParameterPrior, prior_o_segment)
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 97d9b7ae1..8ee7623dc 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -24,9 +24,9 @@ TEST(Problem, create)
ProblemPtr P = Problem::create("POV 3D");
// check double ointers to branches
- ASSERT_EQ(P, P->getHardwarePtr()->getProblem());
- ASSERT_EQ(P, P->getTrajectoryPtr()->getProblem());
- ASSERT_EQ(P, P->getMapPtr()->getProblem());
+ ASSERT_EQ(P, P->getHardware()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getMap()->getProblem());
// check frame structure through the state size
ASSERT_EQ(P->getFrameStructureSize(), 10);
@@ -42,7 +42,7 @@ TEST(Problem, Sensors)
// check pointers
ASSERT_EQ(P, S->getProblem());
- ASSERT_EQ(P->getHardwarePtr(), S->getHardwarePtr());
+ ASSERT_EQ(P->getHardware(), S->getHardware());
}
@@ -51,7 +51,7 @@ TEST(Problem, Processor)
ProblemPtr P = Problem::create("PO 3D");
// check motion processor is null
- ASSERT_FALSE(P->getProcessorMotionPtr());
+ ASSERT_FALSE(P->getProcessorMotion());
// add a motion sensor and processor
SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
@@ -62,12 +62,12 @@ TEST(Problem, Processor)
Sm->addProcessor(Pm);
// check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers
- ASSERT_FALSE(P->getProcessorMotionPtr());
+ ASSERT_FALSE(P->getProcessorMotion());
// set processor motion
P->setProcessorMotion(Pm);
// re-check motion processor IS set by addSensor
- ASSERT_EQ(P->getProcessorMotionPtr(), Pm);
+ ASSERT_EQ(P->getProcessorMotion(), Pm);
}
TEST(Problem, Installers)
@@ -87,16 +87,16 @@ TEST(Problem, Installers)
S->addProcessor(pt);
// check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorMotionPtr());
+ ASSERT_FALSE(P->getProcessorMotion());
// install processor motion
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// check motion processor is set
- ASSERT_TRUE(P->getProcessorMotionPtr());
+ ASSERT_TRUE(P->getProcessorMotion());
// check motion processor is correct
- ASSERT_EQ(P->getProcessorMotionPtr(), pm);
+ ASSERT_EQ(P->getProcessorMotion(), pm);
}
TEST(Problem, SetOrigin_PO_2D)
@@ -109,15 +109,15 @@ TEST(Problem, SetOrigin_PO_2D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardwarePtr()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
// check that the state is correct
ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
- FrameBasePtr F = P->getLastFramePtr();
+ FrameBasePtr F = P->getLastFrame();
ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
CaptureBasePtr C = F->getCaptureList().front();
ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
@@ -126,8 +126,8 @@ TEST(Problem, SetOrigin_PO_2D)
// check that the factor is absolute (no pointers to other F, f, or L)
FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOtherPtr());
- ASSERT_FALSE(c->getLandmarkOtherPtr());
+ ASSERT_FALSE(c->getFrameOther());
+ ASSERT_FALSE(c->getLandmarkOther());
// check that the Feature measurement and covariance are the ones provided
ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
@@ -146,15 +146,15 @@ TEST(Problem, SetOrigin_PO_3D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardwarePtr()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
// check that the state is correct
ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
- FrameBasePtr F = P->getLastFramePtr();
+ FrameBasePtr F = P->getLastFrame();
ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
CaptureBasePtr C = F->getCaptureList().front();
ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
@@ -163,8 +163,8 @@ TEST(Problem, SetOrigin_PO_3D)
// check that the factor is absolute (no pointers to other F, f, or L)
FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOtherPtr());
- ASSERT_FALSE(c->getLandmarkOtherPtr());
+ ASSERT_FALSE(c->getFrameOther());
+ ASSERT_FALSE(c->getLandmarkOther());
// check that the Feature measurement and covariance are the ones provided
ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
@@ -190,7 +190,7 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f2->getType().compare("POV 3D"), 0);
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectoryPtr()->getFrameList().size(), (unsigned int) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (unsigned int) 3);
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 4dff5cb6b..8a966f77b 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -172,7 +172,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
- 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- 1 frame that would be used by future data
*/
- ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3);
+ ASSERT_EQ(problem->getTrajectory()->getFrameList().size(),(unsigned int) 3);
/* if max_time_span == 2, Wolf tree should be
@@ -215,20 +215,20 @@ TEST_F(ProcessorIMUt, interpolate)
// make one step to depart from origin
cap_imu_ptr->setTimeStamp(0.05);
sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
+ Motion mot_ref = problem->getProcessorMotion()->getMotion();
// make two steps, then pretend it's just one
cap_imu_ptr->setTimeStamp(0.10);
sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
+ Motion mot_int_gt = problem->getProcessorMotion()->getMotion();
cap_imu_ptr->setTimeStamp(0.15);
sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
+ Motion mot_final = problem->getProcessorMotion()->getMotion();
mot_final.delta_ = mot_final.delta_integr_;
Motion mot_sec = mot_final;
-// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
+// problem->getProcessorMotion()->getBuffer().print(0,1,1,0);
class P : public wolf::ProcessorIMU
{
@@ -267,8 +267,8 @@ TEST_F(ProcessorIMUt, acc_x)
Vector6s b; b << 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
}
TEST_F(ProcessorIMUt, acc_y)
@@ -293,8 +293,8 @@ TEST_F(ProcessorIMUt, acc_y)
Vector6s b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 1000; //how many ms
@@ -307,8 +307,8 @@ TEST_F(ProcessorIMUt, acc_y)
// advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
x << 0,10,0, 0,0,0,1, 0,20,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
TEST_F(ProcessorIMUt, acc_z)
@@ -330,8 +330,8 @@ TEST_F(ProcessorIMUt, acc_z)
Vector6s b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 50; //how 0.1s
@@ -344,8 +344,8 @@ TEST_F(ProcessorIMUt, acc_z)
// advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
x << 0,0,25, 0,0,0,1, 0,0,10;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
TEST_F(ProcessorIMUt, check_Covariance)
@@ -361,8 +361,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr->setTimeStamp(0.1);
sensor_ptr->process(cap_imu_ptr);
- VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+ VectorXs delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
+// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
@@ -428,8 +428,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
problem->setPrior(x0, P0, t, 0.01);
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
+ problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
+ problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data
@@ -484,8 +484,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6s bias; bias << abx,aby,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
+ problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
+ problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index aba232636..7479ca984 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -89,7 +89,7 @@ TEST(ProcessorBase, KeyFrameCallback)
// problem->print(4,1,1,0);
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 2D")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );
}
}
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 4e2b427fb..a3e18e59c 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -99,10 +99,10 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
F4->addCapture(capture_dummy);
// Add first three states to trajectory
- problem->getTrajectoryPtr()->addFrame(F1);
- problem->getTrajectoryPtr()->addFrame(F2);
- problem->getTrajectoryPtr()->addFrame(F3);
- problem->getTrajectoryPtr()->addFrame(F4);
+ problem->getTrajectory()->addFrame(F1);
+ problem->getTrajectory()->addFrame(F2);
+ problem->getTrajectory()->addFrame(F3);
+ problem->getTrajectory()->addFrame(F4);
// Add same covariances for all states
Eigen::Matrix2s position_covariance_matrix;
@@ -148,7 +148,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
1.2,
sensor_ptr);
// Make 3rd frame last Keyframe
- problem->getTrajectoryPtr()->setLastKeyFramePtr(F3);
+ problem->getTrajectory()->setLastKeyFramePtr(F3);
// trigger search for loopclosure
processor_ptr_point2d->process(incomming_dummy);
@@ -171,17 +171,17 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
position_covariance_matrix << 5.0, 0.0,
0.0, 9.0;
- problem->addCovarianceBlock( F1->getPPtr(), F1->getPPtr(),
+ problem->addCovarianceBlock( F1->getP(), F1->getP(),
position_covariance_matrix);
- problem->addCovarianceBlock( F2->getPPtr(), F2->getPPtr(),
+ problem->addCovarianceBlock( F2->getP(), F2->getP(),
position_covariance_matrix);
- problem->addCovarianceBlock( F3->getPPtr(), F3->getPPtr(),
+ problem->addCovarianceBlock( F3->getP(), F3->getP(),
position_covariance_matrix);
- problem->addCovarianceBlock( F4->getPPtr(), F4->getPPtr(),
+ problem->addCovarianceBlock( F4->getP(), F4->getP(),
position_covariance_matrix);
// Make 3rd frame last Keyframe
- problem->getTrajectoryPtr()->setLastKeyFramePtr(F3);
+ problem->getTrajectory()->setLastKeyFramePtr(F3);
// trigger search for loopclosure
processor_ptr_ellipse2d->process(incomming_dummy);
@@ -193,7 +193,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
// Make 4th frame last Keyframe
- problem->getTrajectoryPtr()->setLastKeyFramePtr(F4);
+ problem->getTrajectory()->setLastKeyFramePtr(F4);
// trigger search for loopclosure again
processor_ptr_ellipse2d->process(incomming_dummy);
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 047e87f40..6819cba18 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -137,11 +137,11 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
proc_trk->process(capt_trk);
- CaptureBasePtr prev = proc_trk->getPrevOriginPtr();
+ CaptureBasePtr prev = proc_trk->getPrevOrigin();
problem->print(2,0,0,0);
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 3D")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 3D")==0 );
}
}
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
index 14b24deb1..9ca4d6571 100644
--- a/test/gtest_sensor_camera.cpp
+++ b/test/gtest_sensor_camera.cpp
@@ -48,17 +48,17 @@ TEST(SensorCamera, Intrinsics_Raw_Rectified)
// default is raw
ASSERT_TRUE(cam->isUsingRawImages());
ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
cam->useRectifiedImages();
ASSERT_FALSE(cam->isUsingRawImages());
ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsicPtr()->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsic()->getState(), 1e-8);
cam->useRawImages();
ASSERT_TRUE(cam->isUsingRawImages());
ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
}
TEST(SensorCamera, Distortion)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index f16fe25a3..75d295515 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -85,7 +85,7 @@ class SolverManagerWrapper : public SolverManager
virtual void addStateBlock(const StateBlockPtr& state_ptr)
{
state_block_fixed_[state_ptr] = state_ptr->isFixed();
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrizationPtr();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
};
virtual void removeStateBlock(const StateBlockPtr& state_ptr)
{
@@ -98,10 +98,10 @@ class SolverManagerWrapper : public SolverManager
};
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
{
- if (state_ptr->getLocalParametrizationPtr() == nullptr)
+ if (state_ptr->getLocalParametrization() == nullptr)
state_block_local_param_.erase(state_ptr);
else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrizationPtr();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
};
};
@@ -111,7 +111,7 @@ TEST(SolverManager, Create)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblemPtr());
+ ASSERT_EQ(P, solver_manager_ptr->getProblem());
}
TEST(SolverManager, AddStateBlock)
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 9d65d8c0f..e3d533a5f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -288,7 +288,7 @@ TEST_F(TrackMatrixTest, snapshotAsList)
* f2 trk 1
*/
- std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapturePtr()); // get track 0 as vector
+ std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector
ASSERT_EQ(lt0.size() , (unsigned int) 2);
ASSERT_EQ(lt0.front(), f0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 4b1a0a242..d77defcbc 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -62,7 +62,7 @@ TEST(TrajectoryBase, ClosestKeyFrame)
{
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
// kf1 kf2 f3 frames
@@ -99,7 +99,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
using std::make_shared;
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
DummyNotificationProcessor N(P);
@@ -136,8 +136,8 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
std::cout << __LINE__ << std::endl;
N.update();
@@ -151,8 +151,8 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
std::cout << __LINE__ << std::endl;
f3->remove(); // F
@@ -162,7 +162,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
f1->remove(); // KF
if (debug) P->print(2,0,0,0);
@@ -182,7 +182,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
using std::make_shared;
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
// kf1 kf2 f3 frames
@@ -196,23 +196,23 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// add frames and keyframes in random order --> keyframes must be sorted after that
T->addFrame(f2); // KF2
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
T->addFrame(f3); // F3
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
T->addFrame(f1); // KF1
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
f3->setKey(); // set KF3
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
T->addFrame(f4);
@@ -221,8 +221,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 2 3 1.5 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f4->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f4->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
f4->setKey();
// Trajectory status:
@@ -230,8 +230,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 1.5 2 3 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
f2->setTimeStamp(4);
// Trajectory status:
@@ -239,8 +239,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 1.5 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
f4->setTimeStamp(0.5);
// Trajectory status:
diff --git a/wolf_scripts/substitution.sh b/wolf_scripts/substitution.sh
index dcc668c9c..3085c1f82 100755
--- a/wolf_scripts/substitution.sh
+++ b/wolf_scripts/substitution.sh
@@ -10,7 +10,7 @@ for file in $(ag 'Ptr\(' . -o); do
# subs_line=${line}s/${subs}/${subs%List}PtrList/gp
# echo $subs_line
# sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
- sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
+ sed -i -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/g' $target
done
# for file in $(ag -l -g constraint $folder); do
--
GitLab