diff --git a/src/capture_IMU.cpp b/src/capture_IMU.cpp
index d497a0f77e1fbf33116f5df2a89603c4d857ca64..90e494dbfbbad1ad538065417af10b47567a47a8 100644
--- a/src/capture_IMU.cpp
+++ b/src/capture_IMU.cpp
@@ -11,9 +11,9 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
const Eigen::Vector6s& _acc_gyro_data,
const Eigen::MatrixXs& _data_cov,
FrameBasePtr _origin_frame_ptr) :
- CaptureMotion(_init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false))
+ CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false))
{
- setType("IMU");
+ //
}
CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
@@ -22,9 +22,9 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
const Eigen::MatrixXs& _data_cov,
const Vector6s& _bias,
FrameBasePtr _origin_frame_ptr) :
- CaptureMotion(_init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false))
+ CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false))
{
- setType("IMU");
+ //
}
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 60b2858cc7a8a202016d217f209c1e6c659cca56..fdc75a7be4916f3c8e379538dac4666673bed08c 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -56,6 +56,8 @@ CaptureBase::CaptureBase(const std::string& _type,
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
updateCalibSize();
+
+ WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
}
diff --git a/src/capture_image.cpp b/src/capture_image.cpp
index a198322d17f4acefc259dd8c6337a23d7519e56a..6a7950d6253696a2e3aa1def2ea67139e99c157e 100644
--- a/src/capture_image.cpp
+++ b/src/capture_image.cpp
@@ -3,7 +3,14 @@
namespace wolf {
CaptureImage::CaptureImage(const TimeStamp& _ts, SensorCameraPtr _camera_ptr, cv::Mat _data_cv) :
- CaptureBase("IMAGE", _ts, _camera_ptr), frame_(_data_cv)
+ CaptureBase("IMAGE", _ts, _camera_ptr),
+ frame_(_data_cv),
+ grid_features_(nullptr),
+ keypoints_(KeyPointVector()),
+ descriptors_(cv::Mat()),
+ matches_from_precedent_(DMatchVector()),
+ matches_normalized_scores_(std::vector<Scalar>()),
+ map_index_to_next_(std::map<int, int>())
{
//
}
diff --git a/src/capture_image.h b/src/capture_image.h
index 312735ff2918447356d06d669e6e5a0aef3502d0..dd5895338e5144879d32d80896e8880c180e7b2a 100644
--- a/src/capture_image.h
+++ b/src/capture_image.h
@@ -7,8 +7,8 @@
#include "sensor_camera.h"
// Vision Utils includes
-#include <vision_utils.h>
-#include <common_class/frame.h>
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/common_class/frame.h>
namespace wolf {
@@ -26,6 +26,14 @@ class CaptureImage : public CaptureBase
protected:
vision_utils::Frame frame_;
+ public:
+ vision_utils::FeatureIdxGridPtr grid_features_;
+ KeyPointVector keypoints_;
+ cv::Mat descriptors_;
+ DMatchVector matches_from_precedent_;
+ std::vector<Scalar> matches_normalized_scores_;
+ std::map<int, int> map_index_to_next_;
+
public:
CaptureImage(const TimeStamp& _ts, SensorCameraPtr _camera_ptr, cv::Mat _data_cv);
virtual ~CaptureImage();
diff --git a/src/capture_motion.cpp b/src/capture_motion.cpp
index a4d74cbfbb67a5f60e9ec5debbcbbf72a6cc83ff..3c7a5158c68c98c1810042b1cdd4a69eb9ba0489 100644
--- a/src/capture_motion.cpp
+++ b/src/capture_motion.cpp
@@ -10,13 +10,14 @@
namespace wolf
{
-CaptureMotion::CaptureMotion(const TimeStamp& _ts,
+CaptureMotion::CaptureMotion(const std::string & _type,
+ const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
Size _delta_size,
Size _delta_cov_size,
FrameBasePtr _origin_frame_ptr) :
- CaptureBase("MOTION", _ts, _sensor_ptr),
+ CaptureBase(_type, _ts, _sensor_ptr),
data_(_data),
data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
@@ -25,7 +26,8 @@ CaptureMotion::CaptureMotion(const TimeStamp& _ts,
//
}
-CaptureMotion::CaptureMotion(const TimeStamp& _ts,
+CaptureMotion::CaptureMotion(const std::string & _type,
+ const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
@@ -35,7 +37,7 @@ CaptureMotion::CaptureMotion(const TimeStamp& _ts,
StateBlockPtr _p_ptr ,
StateBlockPtr _o_ptr ,
StateBlockPtr _intr_ptr ) :
- CaptureBase("MOTION", _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
+ CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
data_(_data),
data_cov_(_data_cov),
buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
diff --git a/src/capture_motion.h b/src/capture_motion.h
index 6cc4f1732280192431e9e37b9b485fca82e30498..056d3c486f8f98aa8fd5d954c408a37eb77291d2 100644
--- a/src/capture_motion.h
+++ b/src/capture_motion.h
@@ -46,13 +46,17 @@ class CaptureMotion : public CaptureBase
// public interface:
public:
- CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr,
+ CaptureMotion(const std::string & _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
Size _delta_size,
Size _delta_cov_size,
FrameBasePtr _origin_frame_ptr);
- CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr,
+ CaptureMotion(const std::string & _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
Size _delta_size,
Size _delta_cov_size,
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index 24e9f2ee280f0e3c272f87c99acb144a2974f511..5d8eabccb3ab7e01ef77e14709778da904e43f97 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -14,9 +14,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
FrameBasePtr _origin_frame_ptr):
- CaptureMotion(_init_ts, _sensor_ptr, _data, 3, 3, _origin_frame_ptr)
+ CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _origin_frame_ptr)
{
- setType("ODOM 2D");
+ //
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
@@ -24,9 +24,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
FrameBasePtr _origin_frame_ptr):
- CaptureMotion(_init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _origin_frame_ptr)
+ CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _origin_frame_ptr)
{
- setType("ODOM 2D");
+ //
}
CaptureOdom2D::~CaptureOdom2D()
diff --git a/src/capture_odom_3D.cpp b/src/capture_odom_3D.cpp
index 232f088ffa9f639e089a5c154342217a5eb80d52..f6ba1dea9e315c88cb36676c89ede40675028169 100644
--- a/src/capture_odom_3D.cpp
+++ b/src/capture_odom_3D.cpp
@@ -14,9 +14,9 @@ CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _data,
FrameBasePtr _origin_frame_ptr):
- CaptureMotion(_init_ts, _sensor_ptr, _data, 7, 6, _origin_frame_ptr)
+ CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _origin_frame_ptr)
{
- setType("ODOM 3D");
+ //
}
CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
@@ -24,9 +24,9 @@ CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
const Eigen::Vector6s& _data,
const Eigen::MatrixXs& _data_cov,
FrameBasePtr _origin_frame_ptr):
- CaptureMotion(_init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _origin_frame_ptr)
+ CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _origin_frame_ptr)
{
- setType("ODOM 3D");
+ //
}
CaptureOdom3D::~CaptureOdom3D()
diff --git a/src/captures/CMakeLists.txt b/src/captures/CMakeLists.txt
index c856a0c217f4f7c1bc631532498358be1a279736..008619d7b78511d7deb0bbd1d9133845b667a140 100644
--- a/src/captures/CMakeLists.txt
+++ b/src/captures/CMakeLists.txt
@@ -5,9 +5,18 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
SET(HDRS_CAPTURE ${HDRS_CAPTURE}
${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.h
${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.h
- PARENT_SCOPE)
+ )
SET(SRCS_CAPTURE ${SRCS_CAPTURE}
${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.cpp
${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.cpp
- PARENT_SCOPE)
+ )
+
+
+
+
+
+
+# These lines always at the end
+SET(HDRS_CAPTURE ${HDRS_CAPTURE} PARENT_SCOPE)
+SET(SRCS_CAPTURE ${SRCS_CAPTURE} PARENT_SCOPE)
diff --git a/src/captures/capture_velocity.cpp b/src/captures/capture_velocity.cpp
index 45384d37e0178ad2a56f0de760181b0d7d9a9fe2..49e5ce8ee313e6d0ac16096841b8a17866ed7902 100644
--- a/src/captures/capture_velocity.cpp
+++ b/src/captures/capture_velocity.cpp
@@ -7,10 +7,10 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
const Eigen::VectorXs& _velocity,
Size _delta_size, Size _delta_cov_size,
FrameBasePtr _origin_frame_ptr) :
- CaptureMotion(_ts, _sensor_ptr, _velocity,
+ CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity,
_delta_size, _delta_cov_size, _origin_frame_ptr)
{
- setType("VELOCITY");
+ //
}
CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
@@ -22,11 +22,11 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
- CaptureMotion(_ts, _sensor_ptr, _velocity, _velocity_cov,
+ CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity, _velocity_cov,
_delta_size, _delta_cov_size, _origin_frame_ptr,
_p_ptr, _o_ptr, _intr_ptr)
{
- setType("VELOCITY");
+ //
}
const Eigen::VectorXs& CaptureVelocity::getVelocity() const
diff --git a/src/captures/capture_wheel_joint_position.cpp b/src/captures/capture_wheel_joint_position.cpp
index 46caa2d95c6f7aaf4d81e29523b5da2a9aa7d3be..a43786df6fd0866088e0e42909acc29a9ad7017c 100644
--- a/src/captures/capture_wheel_joint_position.cpp
+++ b/src/captures/capture_wheel_joint_position.cpp
@@ -8,10 +8,10 @@ CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
const Eigen::VectorXs& _positions,
FrameBasePtr _origin_frame_ptr) :
- CaptureMotion(_ts, _sensor_ptr, _positions, Eigen::Matrix2s::Zero(), 3, 3,
+ CaptureMotion("WHEEL JOINT POSITION", _ts, _sensor_ptr, _positions, Eigen::Matrix2s::Zero(), 3, 3,
_origin_frame_ptr/*, nullptr, nullptr, std::make_shared<StateBlock>(3, false)*/)
{
-// setType("WHEEL JOINT POSITION");
+//
const IntrinsicsDiffDrive intrinsics =
*(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
@@ -31,10 +31,10 @@ CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
- CaptureMotion(_ts, _sensor_ptr, _positions, _positions_cov, 3, 3,
+ CaptureMotion("WHEEL JOINT POSITION", _ts, _sensor_ptr, _positions, _positions_cov, 3, 3,
_origin_frame_ptr, _p_ptr, _o_ptr, _intr_ptr)
{
-// setType("WHEEL JOINT POSITION");
+//
}
Eigen::VectorXs CaptureWheelJointPosition::correctDelta(const VectorXs& _delta,
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 6486f200efd89a492cc0260258a542c2ac50823d..94c3d3dbe01e066205cd6ceb7179c22b1dc1df42 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -49,9 +49,9 @@ public:
ceres::Solver::Summary getSummary();
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
- = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS);
+ = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- virtual void computeCovariances(const StateBlockList& st_list);
+ virtual void computeCovariances(const StateBlockList& st_list) override;
ceres::Solver::Options& getSolverOptions();
diff --git a/src/constraints/CMakeLists.txt b/src/constraints/CMakeLists.txt
index 1caa225ee7e3bcef9d91061097dd443441c160ba..587c9b9d99a1f12f6a6f3035801d7657defa33f7 100644
--- a/src/constraints/CMakeLists.txt
+++ b/src/constraints/CMakeLists.txt
@@ -6,7 +6,14 @@ SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT}
${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_distance_3D.h
${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_trifocal.h
${CMAKE_CURRENT_SOURCE_DIR}/constraint_diff_drive.h
- PARENT_SCOPE)
+ )
SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT}
- PARENT_SCOPE)
+ )
+
+
+# These lines always at the end
+SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} PARENT_SCOPE)
+SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE)
+
+
\ No newline at end of file
diff --git a/src/constraints/constraint_autodiff_trifocal.h b/src/constraints/constraint_autodiff_trifocal.h
index 405066042531bd8f9b508830d42141371470ad4c..ac54629a62fcee98f469cce5b65f3864c3147de1 100644
--- a/src/constraints/constraint_autodiff_trifocal.h
+++ b/src/constraints/constraint_autodiff_trifocal.h
@@ -6,8 +6,8 @@
#include "constraint_autodiff.h"
#include "sensor_camera.h"
-#include <vision_utils/common_class/trifocaltensor.h>
-#include <vision_utils/vision_utils.h>
+#include <common_class/trifocaltensor.h>
+#include <vision_utils.h>
namespace wolf
{
@@ -172,7 +172,7 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
Vector3s rtc = _feature_last_ptr ->getCapturePtr()->getSensorPPtr()->getState();
Quaternions rqc = Quaternions(_feature_last_ptr ->getCapturePtr()->getSensorOPtr()->getState().data() );
- // expectation
+ // expectation // canonical units
vision_utils::TrifocalTensorBase<Scalar> tensor;
Matrix3s c2Ec1;
expectation(wtr1, wqr1,
@@ -181,7 +181,7 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
rtc, rqc,
tensor, c2Ec1);
- // error Jacobians
+ // error Jacobians // canonical units
Matrix<Scalar,3,3> J_e_m1, J_e_m2, J_e_m3;
error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3);
@@ -190,17 +190,21 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
Matrix<Scalar,3,2> J_e_u2 = J_e_m2 * J_m_u;
Matrix<Scalar,3,2> J_e_u3 = J_e_m3 * J_m_u;
- // Error covariances induced by each of the measurement covariance
+ // 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();
- // Total error covariance
+ // Total error covariance // canonical units
Matrix3s Q = Q1 + Q2 + Q3;
- // Sqrt of information matrix
+ // Sqrt of information matrix // canonical units
Eigen::LLT<Eigen::MatrixXs> llt_of_info(Q.inverse()); // Cholesky decomposition
sqrt_information_upper = llt_of_info.matrixU();
+
+ // Re-write info matrix (for debug only)
+ // Scalar pix_noise = 1.0;
+ // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3s::Identity(); // one PLP (2D) and one epipolar (1D) constraint
}
// Destructor
diff --git a/src/examples/ACTIVESEARCH.yaml b/src/examples/ACTIVESEARCH.yaml
index 4ff33b353b3921c223665c78f6d9bb8029377c78..028f69ba6321802009765d346f2e735c6a018990 100644
--- a/src/examples/ACTIVESEARCH.yaml
+++ b/src/examples/ACTIVESEARCH.yaml
@@ -33,7 +33,7 @@ matcher:
algorithm:
type: "ACTIVESEARCH"
- draw results: true
+ draw results: false
grid horiz cells: 12
grid vert cells: 8
separation: 10
diff --git a/src/examples/processor_tracker_feature_trifocal.yaml b/src/examples/processor_tracker_feature_trifocal.yaml
index ec31607cfb6ecef94b4506b3fa6d9d174df1caff..67d403058fa6637a15309cb724524c1f9106778d 100644
--- a/src/examples/processor_tracker_feature_trifocal.yaml
+++ b/src/examples/processor_tracker_feature_trifocal.yaml
@@ -9,6 +9,10 @@ algorithm:
voting active: true
minimum features for keyframe: 40
maximum new features: 40
+ grid horiz cells: 10
+ grid vert cells: 10
+ min response new features: 50
+ max euclidean distance: 20
noise:
pixel noise std: 1 # pixels
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index 5df1a0f01a1c57fa6d0601fb77e9c18d2870344b..5c5672c79a21e9e6b12e082ff5873565b9c3cb52 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -149,7 +149,7 @@ int main(int argc, char** argv)
Problem problem(FRM_PO_2D);
SensorOdom2D* odom_sensor = (SensorOdom2D*)problem.installSensor("ODOM 2D", "odometer", odom_pose, &odom_intrinsics);
ProcessorParamsOdom2D odom_params;
- odom_params.cov_det_th_ = 1;
+ odom_params.cov_det = 1;
odom_params.dist_traveled_th_ = 5;
odom_params.elapsed_time_th_ = 10;
ProcessorOdom2D* odom_processor = (ProcessorOdom2D*)problem.installProcessor("ODOM 2D", "main odometry", odom_sensor, &odom_params);
diff --git a/src/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
index 8be57bd098e7b0f735ff76f6363e0ed730a1e650..8e555cac0ba244dc67bde5d8ce86990672a0a244 100644
--- a/src/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -76,7 +76,7 @@ int main()
cv::KeyPoint kp; kp.pt = {10,20};
cv::Mat desc;
- FeaturePointImagePtr feat_point_image_ptr = std::make_shared<FeaturePointImage>(kp, desc, Eigen::Matrix2s::Identity());
+ FeaturePointImagePtr feat_point_image_ptr = std::make_shared<FeaturePointImage>(kp, 0, desc, Eigen::Matrix2s::Identity());
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)));
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 07d272178f3f622745ff8def9848d2512523a426..821f5bdd8038417b50fa0714e69b68ae58f33ede 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -196,8 +196,13 @@ int main(int argc, char** argv)
const auto scalar_max = std::numeric_limits<Scalar>::max();
- ProcessorParamsBasePtr processor_params =
- std::make_shared<ProcessorParamsDiffDrive>(period_secs/2, scalar_max, scalar_max, scalar_max);
+ ProcessorParamsDiffDrivePtr processor_params = std::make_shared<ProcessorParamsDiffDrive>();
+ processor_params->time_tolerance = period_secs/2;
+ processor_params->angle_turned = scalar_max;
+ processor_params->dist_traveled = scalar_max;
+ processor_params->max_time_span = scalar_max;
+ processor_params->max_buff_length = 999;
+ processor_params->unmeasured_perturbation_std = 0.0001;
SensorBasePtr sensor_ptr =
wolf_problem_ptr_->installSensor("DIFF DRIVE", sensor_name, extrinsics, intrinsics);
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index 7f700f69760cff8b93b06daebc89014dfc73c33a..c5afcc61b462553ad01de0e4f27de31f6854d8ea 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -8,9 +8,9 @@
// Vision utils includes
#include <vision_utils.h>
-#include <vision_utils/sensors.h>
-#include <vision_utils/common_class/buffer.h>
-#include <vision_utils/common_class/frame.h>
+#include <sensors.h>
+#include <common_class/buffer.h>
+#include <common_class/frame.h>
//std includes
#include <ctime>
@@ -75,8 +75,8 @@ int main(int argc, char** argv)
// tracker_params.active_search.grid_width = 12;
// tracker_params.active_search.grid_height = 8;
// tracker_params.active_search.separation = 1;
-// tracker_params.algorithm.max_new_features =0;
-// tracker_params.algorithm.min_features_for_keyframe = 20;
+// tracker_params.max_new_features =0;
+// tracker_params.min_features_for_keyframe = 20;
//
// DetectorDescriptorParamsOrb orb_params;
// orb_params.type = DD_ORB;
diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index d8becd8210492f1d07cf3826d4b29bbaa28fb9d9..2822294e75b479df9f493810c73e28bd558aa4f6 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -22,13 +22,17 @@ int main()
SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
- params_odo->elapsed_time_th_ = 2;
- params_odo->theta_traveled_th_ = M_PI; // 180 degrees turn
+ params_odo->max_time_span = 2;
+ params_odo->angle_turned = M_PI; // 180 degrees turn
ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", sen_odo, params_odo);
prc_odo->setTimeTolerance(0.1);
SensorBasePtr sen_ftr = problem->installSensor ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
- shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(0.5, 7, 4);
+ ProcessorParamsTrackerFeaturePtr params_trk = std::make_shared<ProcessorParamsTrackerFeature>();
+ params_trk->max_new_features = 4;
+ params_trk->min_features_for_keyframe = 7;
+ params_trk->time_tolerance = 0.5;
+ shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(params_trk);
prc_ftr->setName("tracker");
sen_ftr->addProcessor(prc_ftr);
prc_ftr->setTimeTolerance(0.1);
@@ -60,7 +64,7 @@ int main()
cout << "=======================\n>> TIME: " << t.get() << endl;
cout << "Motion-----------------" << endl;
- sen_odo->process(make_shared<CaptureMotion>(t, sen_odo, odo_data, 3, 3, nullptr));
+ sen_odo->process(make_shared<CaptureMotion>("ODOM 2D", t, sen_odo, odo_data, 3, 3, nullptr));
cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 8cfdb9d8822347057fe30751058f253b5ea340b9..b7c2c9a9d7313387990b6a9b1b02de18707e5b37 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -66,7 +66,7 @@ int main (int argc, char** argv)
Scalar dyaw = 2*M_PI/5;
Vector6s data((Vector6s() << dx*cos(dyaw/2),dx*sin(dyaw/2),0,0,0,dyaw).finished()); // will integrate this data repeatedly
- CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>(TimeStamp(0), sen, data, 7, 6, nullptr);
+ CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>("ODOM 3D", TimeStamp(0), sen, data, 7, 6, nullptr);
cout << "t: " << std::setprecision(2) << 0 << " \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index fcf29c545f2cdd837f5190f0c123e8bde25835d7..943e1801078adc795409deebe50cb9b49f16dabb 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -31,7 +31,11 @@ int main()
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- shared_ptr<ProcessorTrackerFeatureDummy> processor_ptr_ = make_shared<ProcessorTrackerFeatureDummy>(.25, 7, 4);
+ ProcessorParamsTrackerFeaturePtr params_trk = std::make_shared<ProcessorParamsTrackerFeature>();
+ params_trk->max_new_features = 4;
+ params_trk->min_features_for_keyframe = 7;
+ params_trk->time_tolerance = 0.25;
+ shared_ptr<ProcessorTrackerFeatureDummy> processor_ptr_ = make_shared<ProcessorTrackerFeatureDummy>(params_trk);
wolf_problem_ptr_->addSensor(sensor_ptr_);
sensor_ptr_->addProcessor(processor_ptr_);
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index 6f828298d8147e1bba28f373558d8eda96c04276..bbf3e4132ed3dc6cae6342908a95d1c6382fe559 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -66,7 +66,11 @@ int main()
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(.25, 5);
+ ProcessorParamsTrackerLandmarkPtr params_trk = std::make_shared<ProcessorParamsTrackerLandmark>();
+ params_trk->max_new_features = 5;
+ params_trk->min_features_for_keyframe = 7;
+ params_trk->time_tolerance = 0.25;
+ std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk);
wolf_problem_ptr_->addSensor(sensor_ptr_);
sensor_ptr_->addProcessor(processor_ptr_);
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index 4060656a2ac762d4bb2ac4904f047cf9ccccd163..bbd3b8ca6a72516d50e7fd4da4d70eb30aed8e45 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -1,21 +1,24 @@
//std
#include <iostream>
-#include "../capture_pose.h"
+#include "processor_tracker_landmark_image.h"
+
//Wolf
#include "wolf.h"
#include "problem.h"
#include "state_block.h"
#include "processor_odom_3D.h"
#include "sensor_odom_3D.h"
+#include "sensor_camera.h"
+#include "capture_image.h"
+#include "capture_pose.h"
#include "ceres_wrapper/ceres_manager.h"
// Vision utils includes
#include <vision_utils.h>
-#include <vision_utils/sensors.h>
-#include <vision_utils/common_class/buffer.h>
-#include <vision_utils/common_class/frame.h>
-#include "../processor_tracker_landmark_image.h"
+#include <sensors.h>
+#include <common_class/buffer.h>
+#include <common_class/frame.h>
using Eigen::Vector3s;
using Eigen::Vector4s;
@@ -107,7 +110,7 @@ int main(int argc, char** argv)
// running CAPTURES preallocated
CaptureImagePtr image;
Vector6s data(Vector6s::Zero()); // will integrate this data repeatedly
- CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>(t, sensor_odom, data, 7, 6, nullptr);
+ CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>("IMAGE", t, sensor_odom, data, 7, 6, nullptr);
//=====================================================
//=====================================================
diff --git a/src/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
index dc63b8e2f99115a853d97a1ddda1826b8807d64a..7945c939f82355435c422c691433d9b48937ccda 100644
--- a/src/examples/test_projection_points.cpp
+++ b/src/examples/test_projection_points.cpp
@@ -1,6 +1,6 @@
// Vision utils
-#include <vision_utils.h>
+#include <vision_utils/vision_utils.h>
//std includes
#include <iostream>
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 0f4d3f8eb8d7af228db27d3535c808909a780356..6a3279326aafb627e7c12a57b39aef26fb93ad4d 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -16,7 +16,7 @@
#include "ceres_wrapper/ceres_manager.h"
// Vision utils
-#include <vision_utils.h>
+#include <vision_utils/vision_utils.h>
/**
* This test simulates the following situation:
@@ -144,15 +144,15 @@ int main(int argc, char** argv)
cv::Mat desc;
cv::KeyPoint kp_0;
- FeaturePointImagePtr feat_0 = std::make_shared<FeaturePointImage>(kp_0, desc, Eigen::Matrix2s::Identity());
+ FeaturePointImagePtr feat_0 = std::make_shared<FeaturePointImage>(kp_0, 0, desc, Eigen::Matrix2s::Identity());
image_0->addFeature(feat_0);
cv::KeyPoint kp_1;
- FeaturePointImagePtr feat_1 = std::make_shared<FeaturePointImage>(kp_1, desc, Eigen::Matrix2s::Identity());
+ FeaturePointImagePtr feat_1 = std::make_shared<FeaturePointImage>(kp_1, 0, desc, Eigen::Matrix2s::Identity());
image_1->addFeature(feat_1);
cv::KeyPoint kp_2;
- FeaturePointImagePtr feat_2 = std::make_shared<FeaturePointImage>(kp_2, desc, Eigen::Matrix2s::Identity());
+ FeaturePointImagePtr feat_2 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity());
image_2->addFeature(feat_2);
// Landmark--------------------
@@ -193,10 +193,10 @@ int main(int argc, char** argv)
//======== now we want to estimate a new lmk ===============
//
// Features -----------------
- FeaturePointImagePtr feat_3 = std::make_shared<FeaturePointImage>(kp_1, desc, Eigen::Matrix2s::Identity());
+ FeaturePointImagePtr feat_3 = std::make_shared<FeaturePointImage>(kp_1, 0, desc, Eigen::Matrix2s::Identity());
image_1->addFeature(feat_3);
- FeaturePointImagePtr feat_4 = std::make_shared<FeaturePointImage>(kp_2, desc, Eigen::Matrix2s::Identity());
+ FeaturePointImagePtr feat_4 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity());
image_2->addFeature(feat_4);
diff --git a/src/examples/test_tracker_ORB.cpp b/src/examples/test_tracker_ORB.cpp
index 6ad9ec638e5e2985b248322817280342fb927e3e..f695a3cb4d7bd0c2e43974c1aef81328846306fe 100644
--- a/src/examples/test_tracker_ORB.cpp
+++ b/src/examples/test_tracker_ORB.cpp
@@ -3,9 +3,12 @@
// Vision utils
#include <vision_utils.h>
-#include <vision_utils/sensors.h>
-#include <vision_utils/common_class/buffer.h>
-#include <vision_utils/common_class/frame.h>
+#include <sensors.h>
+#include <common_class/buffer.h>
+#include <common_class/frame.h>
+#include <detectors/orb/detector_orb.h>
+#include <descriptors/orb/descriptor_orb.h>
+#include <matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h>
//Wolf
#include "../processor_tracker_landmark_image.h"
@@ -137,7 +140,7 @@ int main(int argc, char** argv)
if(keypoints.size() != 0)
{
- mat_ptr->match(target_descriptor, descriptors, cv_matches);
+ mat_ptr->match(target_descriptor, descriptors, des_ptr->getSize(), cv_matches);
Scalar normalized_score = 1 - (Scalar)(cv_matches[0].distance)/(des_ptr->getSize()*8);
std::cout << "pixel: " << keypoints[cv_matches[0].trainIdx].pt + roi_up_left_corner << std::endl;
std::cout << "normalized score: " << normalized_score << std::endl;
diff --git a/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
index 9a74ae4bcb4abc75bf2b0297de8d64f4648afa35..2ca6eb55eca8c9d4bfddbd6a1b8c84ae5016999a 100644
--- a/src/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -89,8 +89,8 @@ int main()
p.image.height = img["height"].as<unsigned int>();
Node alg = params["algorithm"];
- p.algorithm.max_new_features = alg["maximum new features"].as<unsigned int>();
- p.algorithm.min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
+ p.max_new_features = alg["maximum new features"].as<unsigned int>();
+ p.min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
}
diff --git a/src/feature_point_image.h b/src/feature_point_image.h
index a1a373378d922aad93dd616d00468bcf9bd23760..43cad5398e235045b0914102dd41be749f63d766 100644
--- a/src/feature_point_image.h
+++ b/src/feature_point_image.h
@@ -17,6 +17,7 @@ class FeaturePointImage : public FeatureBase
{
private:
cv::KeyPoint keypoint_; ///< Warning: every write operation to this member needs to write measurement_. See setKeypoint() as an example.
+ unsigned int index_keypoint_;
cv::Mat descriptor_;
bool is_known_;
Scalar score_;
@@ -26,10 +27,12 @@ class FeaturePointImage : public FeatureBase
/// Constructor from Eigen measured pixel
FeaturePointImage(const Eigen::Vector2s & _measured_pixel,
+ const unsigned int& _index_keypoint,
const cv::Mat& _descriptor,
const Eigen::Matrix2s& _meas_covariance) :
FeatureBase("POINT IMAGE", _measured_pixel, _meas_covariance),
keypoint_(_measured_pixel(0), _measured_pixel(1), 1), // Size 1 is a dummy value
+ index_keypoint_(_index_keypoint),
descriptor_( _descriptor),
is_known_(false),
score_(0)
@@ -40,10 +43,12 @@ class FeaturePointImage : public FeatureBase
/// Constructor from OpenCV measured keypoint
FeaturePointImage(const cv::KeyPoint& _keypoint,
+ const unsigned int& _index_keypoint,
const cv::Mat& _descriptor,
const Eigen::Matrix2s& _meas_covariance) :
FeatureBase("POINT IMAGE", Eigen::Vector2s::Zero(), _meas_covariance),
keypoint_(_keypoint),
+ index_keypoint_(_index_keypoint),
descriptor_(_descriptor),
is_known_(false),
score_(0)
@@ -60,6 +65,9 @@ class FeaturePointImage : public FeatureBase
const cv::Mat& getDescriptor() const;
void setDescriptor(const cv::Mat& _descriptor);
+ size_t getIndexKeyPoint() const
+ { return index_keypoint_; }
+
bool isKnown();
void setIsKnown(bool _is_known);
diff --git a/src/features/CMakeLists.txt b/src/features/CMakeLists.txt
index ba959f48215e7310f605e8e57673b3dc64ef6af4..dd0b75aa447b2bd2a09c127e8b7a4abc0064ee58 100644
--- a/src/features/CMakeLists.txt
+++ b/src/features/CMakeLists.txt
@@ -4,8 +4,15 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
SET(HDRS_FEATURE ${HDRS_FEATURE}
${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.h
- PARENT_SCOPE)
+ )
SET(SRCS_FEATURE ${SRCS_FEATURE}
${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp
- PARENT_SCOPE)
+ )
+
+
+
+# These lines always at the end
+SET(HDRS_FEATURE ${HDRS_FEATURE} PARENT_SCOPE)
+SET(SRCS_FEATURE ${SRCS_FEATURE} PARENT_SCOPE)
+
\ No newline at end of file
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 70016a8e60045b2edccaa871e1efc85758d3ca8c..53bd22e246d5ca0eb51d32802614f868ca5d075a 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -24,14 +24,8 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
state_block_vec_[1] = _o_ptr;
state_block_vec_[2] = _v_ptr;
-// if ( isKey() )
-// {
-// WOLF_DEBUG("New KF", this->id() );
-// }
-// else
-// {
-// WOLF_DEBUG("New F", this->id() );
-// }
+ if ( isKey() )
+ registerNewStateBlocks();
}
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
@@ -47,19 +41,13 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
state_block_vec_[1] = _o_ptr;
state_block_vec_[2] = _v_ptr;
-// if ( isKey() )
-// {
-// WOLF_DEBUG("New KF", this->id() );
-// }
-// else
-// {
-// WOLF_DEBUG("New F", this->id() );
-// }
+ if ( isKey() )
+ registerNewStateBlocks();
}
FrameBase::~FrameBase()
{
- if (type_ == KEY_FRAME)
+ if ( isKey() )
removeStateBlocks();
}
@@ -88,7 +76,7 @@ void FrameBase::remove()
}
// Remove Frame State Blocks
- if (type_ == KEY_FRAME)
+ if ( isKey() )
removeStateBlocks();
if (getTrajectoryPtr()->getLastKeyFramePtr()->id() == this_F->id())
diff --git a/src/hello_wolf/CMakeLists.txt b/src/hello_wolf/CMakeLists.txt
index 9f2270389436071e20df00d288e5734c2e52ba5c..96b4a4b48ba807ce96c9e2e639bb51093ad29b47 100644
--- a/src/hello_wolf/CMakeLists.txt
+++ b/src/hello_wolf/CMakeLists.txt
@@ -10,7 +10,6 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.h
- PARENT_SCOPE
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
@@ -20,9 +19,16 @@ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
- PARENT_SCOPE
)
ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME})
-
\ No newline at end of file
+
+
+
+
+# These lines always at the end
+SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE )
+SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE )
+
+
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index 653fdcc9ebd696d9774a97a60cb66d0016500227..416eca9058efde6d43073ba1e94c4af493166e65 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -123,11 +123,11 @@ int main()
// processor odometer 2D
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
- params_odo->elapsed_time_th_ = 999;
- params_odo->dist_traveled_th_ = 0.95; // Will make KFs automatically every 1m displacement
- params_odo->theta_traveled_th_ = 999;
- params_odo->cov_det_th_ = 999;
- params_odo->unmeasured_perturbation_std_ = 0.001;
+ params_odo->max_time_span = 999;
+ params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement
+ params_odo->angle_turned = 999;
+ params_odo->cov_det = 999;
+ params_odo->unmeasured_perturbation_std = 0.001;
ProcessorBasePtr processor = problem->installProcessor("ODOM 2D", "processor odo", sensor_odo, params_odo);
ProcessorOdom2DPtr processor_odo = std::static_pointer_cast<ProcessorOdom2D>(processor);
diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp
index cb17c8a8a7713b2c394084f8bfea20b307e13583..66160b20f16cb68713ef03e9dbd1cc516445d4b6 100644
--- a/src/hello_wolf/processor_range_bearing.cpp
+++ b/src/hello_wolf/processor_range_bearing.cpp
@@ -14,8 +14,8 @@
namespace wolf
{
-ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, const Scalar& _time_tolerance) :
- ProcessorBase("RANGE BEARING", _time_tolerance)
+ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) :
+ ProcessorBase("RANGE BEARING", _params)
{
H_r_s = transform(_sensor_ptr->getPPtr()->getState(), _sensor_ptr->getOPtr()->getState());
}
@@ -26,7 +26,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
if ( !kf_pack_buffer_.empty() )
{
// Select using incoming_ptr
- KFPackPtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), time_tolerance_ );
+ KFPackPtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
if (pack!=nullptr)
keyFrameCallback(pack->key_frame,pack->time_tolerance);
@@ -38,7 +38,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
// 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below)
auto kf = getProblem()->closestKeyFrameToTimeStamp(capture->getTimeStamp());
- assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < time_tolerance_) && "Could not find a KF close enough to _capture!");
+ assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
// 2. create Capture
auto cap = std::make_shared<CaptureRangeBearing>(capture->getTimeStamp(),
@@ -111,7 +111,7 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
// construct processor
- ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params->time_tolerance);
+ ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
// setup processor
prc->setName(_unique_name);
diff --git a/src/hello_wolf/processor_range_bearing.h b/src/hello_wolf/processor_range_bearing.h
index 881c7d99626cb4df778013289a81fd6ce7217c8a..5a5f1c82fe5f49946a5a212c13bc6068088e9ef0 100644
--- a/src/hello_wolf/processor_range_bearing.h
+++ b/src/hello_wolf/processor_range_bearing.h
@@ -33,7 +33,7 @@ class ProcessorRangeBearing : public ProcessorBase
public:
typedef Eigen::Transform<Scalar, 2, Eigen::Affine> Trf;
- ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, const Scalar& _time_tolerance = 0);
+ ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params);
virtual ~ProcessorRangeBearing() {/* empty */}
virtual void configure(SensorBasePtr _sensor) override { }
diff --git a/src/landmark_AHP.h b/src/landmark_AHP.h
index 6618c58934bc4e9e614fd7ab4410c1ce54437caa..ad55a07244797f24a937e324de69a4926da412b0 100644
--- a/src/landmark_AHP.h
+++ b/src/landmark_AHP.h
@@ -8,7 +8,7 @@
#include <yaml-cpp/yaml.h>
// Vision utils
-#include <vision_utils.h>
+#include <vision_utils/vision_utils.h>
namespace wolf {
diff --git a/src/landmark_point_3D.h b/src/landmark_point_3D.h
index 1d39422edcc4daf77a8815ec2080ab6acd897fee..cb29852b8b05a04220cbe4703bffa12bf8cdd5c9 100644
--- a/src/landmark_point_3D.h
+++ b/src/landmark_point_3D.h
@@ -6,7 +6,7 @@
#include "landmark_base.h"
// Vision Utils includes
-#include <vision_utils.h>
+#include <vision_utils/vision_utils.h>
namespace wolf {
diff --git a/src/problem.cpp b/src/problem.cpp
index 2bf9ad01fa944fe8ae57dad5a325c550dff4285d..f0d56b42104eeb76aa6c6d0eb9a981310947c947 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -36,9 +36,9 @@ Problem::Problem(const std::string& _frame_structure) :
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
processor_motion_ptr_(),
- prior_is_set_(false),
state_size_(0),
- state_cov_size_(0)
+ state_cov_size_(0),
+ prior_is_set_(false)
{
if (_frame_structure == "PO 2D")
{
@@ -63,9 +63,9 @@ Problem::Problem(const std::string& _frame_structure) :
void Problem::setup()
{
- hardware_ptr_->setProblem(shared_from_this());
- trajectory_ptr_->setProblem(shared_from_this());
- map_ptr_->setProblem(shared_from_this());
+ hardware_ptr_ -> setProblem(shared_from_this());
+ trajectory_ptr_-> setProblem(shared_from_this());
+ map_ptr_ -> setProblem(shared_from_this());
}
ProblemPtr Problem::create(const std::string& _frame_structure)
@@ -287,18 +287,15 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size");
if (processor_motion_ptr_ != nullptr)
- {
processor_motion_ptr_->getState(_ts, state);
- }
+
else
{
FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
if (closest_frame != nullptr)
- {
closest_frame->getState(state);
- }else{
+ else
state = zeroState();
- }
}
}
@@ -316,7 +313,7 @@ Size Problem::getFrameStructureSize() const
void Problem::getFrameStructureSize(Size& _x_size, Size& _cov_size) const
{
- _x_size = state_size_;
+ _x_size = state_size_;
_cov_size = state_cov_size_;
}
@@ -334,6 +331,11 @@ Eigen::VectorXs Problem::zeroState()
bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
{
+ // This should implement a black list of processors that have forbidden keyframe creation
+ // This decision is made at problem level, not at processor configuration level.
+ // If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
+
+ // Currently allowing all processors to vote:
return true;
}
diff --git a/src/problem.h b/src/problem.h
index ac69bd6c37a5d18401344bdc973bc827ae51f72b..bd02aff609d3267c59e2ae51fc7efb30c5dbe29b 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -50,9 +50,9 @@ class Problem : public std::enable_shared_from_this<Problem>
StateBlockList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
std::list<ConstraintNotification> constraint_notification_list_;
- bool prior_is_set_;
Size state_size_, state_cov_size_;
StateBlockList notified_state_block_list_;
+ bool prior_is_set_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure); // USE create() below !!
@@ -199,18 +199,17 @@ class Problem : public std::enable_shared_from_this<Problem>
FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
const TimeStamp& _time_stamp);
- // State getters
- Eigen::VectorXs getCurrentState();
- void getCurrentState(Eigen::VectorXs& state);
- void getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& _ts);
- Eigen::VectorXs getState(const TimeStamp& _ts);
- void getState(const TimeStamp& _ts, Eigen::VectorXs& state);
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
+ // Frame getters
+ FrameBasePtr getLastFramePtr ( );
+ FrameBasePtr getLastKeyFramePtr ( );
+ FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
- // Zero state provider
- Eigen::VectorXs zeroState();
/** \brief Give the permission to a processor to create a new keyFrame
+ *
+ * This should implement a black list of processors that have forbidden keyframe creation.
+ * - This decision is made at problem level, not at processor configuration level.
+ * - Therefore, if you want to permanently configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
*/
bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
@@ -222,14 +221,15 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorBasePtr _processor_ptr, //
const Scalar& _time_tolerance);
- /** \brief Returns a pointer to last frame
- **/
- FrameBasePtr getLastFramePtr();
-
- /** \brief Returns a pointer to last key frame
- */
- FrameBasePtr getLastKeyFramePtr();
-
+ // State getters
+ Eigen::VectorXs getCurrentState ( );
+ void getCurrentState (Eigen::VectorXs& state);
+ void getCurrentStateAndStamp (Eigen::VectorXs& state, TimeStamp& _ts);
+ Eigen::VectorXs getState (const TimeStamp& _ts);
+ void getState (const TimeStamp& _ts, Eigen::VectorXs& state);
+ // Zero state provider
+ Eigen::VectorXs zeroState ( );
+ bool priorIsSet() const;
@@ -308,10 +308,6 @@ class Problem : public std::enable_shared_from_this<Problem>
bool state_blocks = false);
bool check(int verbose_level = 0);
- bool priorIsSet() const
- {
- return prior_is_set_;
- }
};
} // namespace wolf
@@ -321,6 +317,11 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
+inline bool Problem::priorIsSet() const
+{
+ return prior_is_set_;
+}
+
inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
{
return processor_motion_ptr_;
diff --git a/src/processor_GPS.cpp b/src/processor_GPS.cpp
index 21940ef5ef058c3cfb06bfd0c688386e4dac7e37..9cc147a4927f2b77eabd18f43053c5779f134207 100644
--- a/src/processor_GPS.cpp
+++ b/src/processor_GPS.cpp
@@ -11,8 +11,8 @@
namespace wolf
{
-ProcessorGPS::ProcessorGPS(Scalar time_tolerance_) :
- ProcessorBase("GPS", time_tolerance_),
+ProcessorGPS::ProcessorGPS(ProcessorParamsBasePtr _params) :
+ ProcessorBase("GPS", _params),
capture_gps_ptr_(nullptr)
{
gps_covariance_ = 10;
@@ -61,7 +61,7 @@ void ProcessorGPS::keyFrameCallback(FrameBasePtr, const Scalar& _time_tol)
ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
{
- ProcessorGPSPtr prc_ptr = std::make_shared<ProcessorGPS>(_params->time_tolerance);
+ ProcessorGPSPtr prc_ptr = std::make_shared<ProcessorGPS>(_params);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor_GPS.h b/src/processor_GPS.h
index bd14aa018fcdaa544507cde0f3bbcd7e6556b3ff..a490bd2c1df80b40f6719a1266b705f154c716c3 100644
--- a/src/processor_GPS.h
+++ b/src/processor_GPS.h
@@ -24,7 +24,7 @@ class ProcessorGPS : public ProcessorBase
Scalar gps_covariance_;
public:
- ProcessorGPS(Scalar time_tolerance_);
+ ProcessorGPS(ProcessorParamsBasePtr _params);
virtual ~ProcessorGPS();
virtual void configure(SensorBasePtr _sensor) { };
virtual void init(CaptureBasePtr _capture_ptr);
diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp
index a3c926185c078a7775173cbe1f42d458d0bf08bb..75c30f1d9ba4070579953c62b5590e887755175c 100644
--- a/src/processor_IMU.cpp
+++ b/src/processor_IMU.cpp
@@ -3,19 +3,14 @@
namespace wolf {
-ProcessorIMU::ProcessorIMU(const ProcessorParamsIMU& _params) :
- ProcessorMotion("IMU", _params.time_tolerance, 10, 10, 9, 6, 6),
- max_time_span_ (_params.max_time_span ),
- max_buff_length_(_params.max_buff_length ),
- dist_traveled_ (_params.dist_traveled ),
- angle_turned_ (_params.angle_turned )
+ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) :
+ ProcessorMotion("IMU", 10, 10, 9, 6, 6, _params_motion_IMU),
+ params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU))
{
// Set constant parts of Jacobians
jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros
jacobian_delta_.setIdentity(9,9); //
jacobian_calib_.setZero(9,6);
-
- setVotingActive(_params.voting_active );
}
ProcessorIMU::~ProcessorIMU()
@@ -25,22 +20,15 @@ ProcessorIMU::~ProcessorIMU()
ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
{
+ std::shared_ptr<ProcessorParamsIMU> prc_imu_params;
if (_params)
- {
- // cast inputs to the correct type
- std::shared_ptr<ProcessorParamsIMU> prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
-
- ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>(*prc_imu_params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
- }
+ prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
else
- {
- ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>();
+ prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_ptr->setName(_unique_name);
- return prc_ptr;
- }
+ ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>(prc_imu_params);
+ prc_ptr->setName(_unique_name);
+ return prc_ptr;
}
bool ProcessorIMU::voteForKeyFrame()
@@ -48,20 +36,20 @@ bool ProcessorIMU::voteForKeyFrame()
if(!isVotingActive())
return false;
// time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > max_time_span_)
+ if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().get().size() > max_buff_length_)
+ if (getBuffer().get().size() > params_motion_IMU_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer length" );
return true;
}
// angle turned
- Scalar angle = 2.0 * asin(delta_integrated_.segment(4,3).norm());
- if (angle > angle_turned_)
+ Scalar angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
+ if (angle > params_motion_IMU_->angle_turned)
{
WOLF_DEBUG( "PM: vote: angle turned" );
return true;
diff --git a/src/processor_IMU.h b/src/processor_IMU.h
index 1828aab3650e7d9db508f9db58b7c890fd0eeb9f..e3810a975d66d44f859395b5e6d9bb2044ac1f16 100644
--- a/src/processor_IMU.h
+++ b/src/processor_IMU.h
@@ -10,12 +10,9 @@
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
-struct ProcessorParamsIMU : public ProcessorParamsBase
+struct ProcessorParamsIMU : public ProcessorParamsMotion
{
- Scalar max_time_span = 0.5;
- Size max_buff_length = 10;
- Scalar dist_traveled = 5;
- Scalar angle_turned = 0.5;
+ //
};
WOLF_PTR_TYPEDEFS(ProcessorIMU);
@@ -23,7 +20,7 @@ WOLF_PTR_TYPEDEFS(ProcessorIMU);
//class
class ProcessorIMU : public ProcessorMotion{
public:
- ProcessorIMU(const ProcessorParamsIMU& _params = ProcessorParamsIMU());
+ ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU);
virtual ~ProcessorIMU();
virtual void configure(SensorBasePtr _sensor) override { };
@@ -64,21 +61,16 @@ class ProcessorIMU : public ProcessorMotion{
CaptureBasePtr _capture_origin) override;
protected:
+ ProcessorParamsIMUPtr params_motion_IMU_;
// keyframe voting parameters
- Scalar max_time_span_; // maximum time between keyframes
- Size max_buff_length_;// maximum buffer size before keyframe
- Scalar dist_traveled_; // maximum linear motion between keyframes
- Scalar angle_turned_; // maximum rotation between keyframes
+// Scalar max_time_span_; // maximum time between keyframes
+// Size max_buff_length_;// maximum buffer size before keyframe
+// Scalar dist_traveled_; // maximum linear motion between keyframes
+// Scalar angle_turned_; // maximum rotation between keyframes
public:
- //getters
- Scalar getMaxTimeSpan() const;
- Scalar getMaxBuffLength() const;
- Scalar getDistTraveled() const;
- Scalar getAngleTurned() const;
-
//for factory
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
};
@@ -103,26 +95,6 @@ inline Eigen::VectorXs ProcessorIMU::deltaZero() const
return (Eigen::VectorXs(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
}
-inline Scalar ProcessorIMU::getMaxTimeSpan() const
-{
- return max_time_span_;
-}
-
-inline Scalar ProcessorIMU::getMaxBuffLength() const
-{
- return max_buff_length_;
-}
-
-inline Scalar ProcessorIMU::getDistTraveled() const
-{
- return dist_traveled_;
-}
-
-inline Scalar ProcessorIMU::getAngleTurned() const
-{
- return angle_turned_;
-}
-
} // namespace wolf
#endif // PROCESSOR_IMU_H
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index f68ef9b9f73d3fc9d84ce925e101a8b6c422959e..ed77aab55d10bf08a3bb155d58c182c31ee7b1a8 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -7,13 +7,12 @@ namespace wolf {
unsigned int ProcessorBase::processor_id_count_ = 0;
-ProcessorBase::ProcessorBase(const std::string& _type, const Scalar& _time_tolerance) :
- NodeBase("PROCESSOR", _type),
+ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params) :
+ NodeBase("PROCESSOR", _type, _params->name),
processor_id_(++processor_id_count_),
- time_tolerance_(_time_tolerance),
+ params_(_params),
sensor_ptr_(),
- is_removing_(false),
- voting_active_(true)
+ is_removing_(false)
{
// WOLF_DEBUG("constructed +p" , id());
}
@@ -48,7 +47,7 @@ FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _captur
void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
{
- WOLF_DEBUG("P", isMotion() ? "M" : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
+ WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
if (_keyframe_ptr != nullptr)
kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
}
diff --git a/src/processor_base.h b/src/processor_base.h
index d63edd93cdf79cb5381531f669fc6e5635bcfb39..0231e5465f1ed575f8a01b37e8a15ad3fa905507 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -141,19 +141,18 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
{
protected:
unsigned int processor_id_;
- Scalar time_tolerance_; ///< self time tolerance for adding a capture into a frame
+ ProcessorParamsBasePtr params_;
KFPackBuffer kf_pack_buffer_;
private:
SensorBaseWPtr sensor_ptr_;
bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
- bool voting_active_; ///< A flag for allowing the processor to vote for KF or not.
static unsigned int processor_id_count_;
public:
- ProcessorBase(const std::string& _type, const Scalar& _time_tolerance);
+ ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params);
virtual ~ProcessorBase();
virtual void configure(SensorBasePtr _sensor) = 0;
void remove();
@@ -209,12 +208,12 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
inline bool ProcessorBase::isVotingActive() const
{
- return voting_active_;
+ return params_->voting_active;
}
inline void ProcessorBase::setVotingActive(bool _voting_active)
{
- voting_active_ = _voting_active;
+ params_->voting_active = _voting_active;
}
}
@@ -246,7 +245,7 @@ inline const SensorBasePtr ProcessorBase::getSensorPtr() const
inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
{
- time_tolerance_ = _time_tolerance;
+ params_->time_tolerance = _time_tolerance;
}
inline KFPackBuffer::KFPackBuffer(void)
diff --git a/src/processor_capture_holder.cpp b/src/processor_capture_holder.cpp
index 398f829aa397888878ed7a0ca4fee73080740315..10ded0c0b2855abb81fc50b560c161cc18772fd5 100644
--- a/src/processor_capture_holder.cpp
+++ b/src/processor_capture_holder.cpp
@@ -10,9 +10,10 @@
namespace wolf {
-ProcessorCaptureHolder::ProcessorCaptureHolder(const Scalar& _buffer_size) :
- ProcessorBase("CAPTURE HOLDER", _buffer_size/2.),
- buffer_(_buffer_size)
+ProcessorCaptureHolder::ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder) :
+ ProcessorBase("CAPTURE HOLDER", _params_capture_holder),
+ params_capture_holder_(_params_capture_holder),
+ buffer_(_params_capture_holder->buffer_size)
{
//
}
@@ -136,7 +137,7 @@ ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name,
if (params == nullptr)
params = std::make_shared<ProcessorParamsCaptureHolder>();
- ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params->buffer_size_);
+ ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
diff --git a/src/processor_capture_holder.h b/src/processor_capture_holder.h
index 8fa402bcf633df0780285cdf824d07ba953d6043..b79743f45e763ca630fa4fa7de31fb106cb16fd6 100644
--- a/src/processor_capture_holder.h
+++ b/src/processor_capture_holder.h
@@ -23,7 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
*/
struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
{
- Scalar buffer_size_ = 30;
+ Scalar buffer_size = 30;
};
/**
@@ -33,7 +33,7 @@ class ProcessorCaptureHolder : public ProcessorBase
{
public:
- ProcessorCaptureHolder(const Scalar& _buffer_size = 1);
+ ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder);
virtual ~ProcessorCaptureHolder() = default;
virtual void configure(SensorBasePtr _sensor) override { };
@@ -59,6 +59,7 @@ public:
protected:
+ ProcessorParamsCaptureHolderPtr params_capture_holder_;
CaptureBuffer buffer_;
public:
diff --git a/src/processor_frame_nearest_neighbor_filter.cpp b/src/processor_frame_nearest_neighbor_filter.cpp
index 8d23b6d4e7270db98b84778abdcf610815901d74..f705e368feebebd6991219a2472619f6cbb23b2a 100644
--- a/src/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor_frame_nearest_neighbor_filter.cpp
@@ -3,18 +3,18 @@
namespace wolf
{
-ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(const Params& _params):
- ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params.time_tolerance),
- params_(_params)
+ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF):
+ ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params_NNF),
+ params_NNF(_params_NNF)
{
// area of ellipse based on the Chi-Square Probabilities
// https://people.richland.edu/james/lecture/m170/tbl-chi.html
// cover both 2D and 3D cases
- if(params_.distance_type_ == DistanceType::LC_POINT_ELLIPSE ||
- params_.distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE)
+ if(params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSE ||
+ params_NNF->distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE)
{
- switch ((int)(params_.probability_*100))
+ switch ((int)(params_NNF->probability_*100))
{
case 900: area_ = 4.605; // 90% probability
break;
@@ -26,10 +26,10 @@ ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(const P
break;
}
}
- if (params_.distance_type_ == DistanceType::LC_POINT_ELLIPSOID ||
- params_.distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID)
+ if (params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSOID ||
+ params_NNF->distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID)
{
- switch ((int)(params_.probability_*100))
+ switch ((int)(params_NNF->probability_*100))
{
case 900: area_ = 6.251; // 90% probability
break;
@@ -97,7 +97,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
// check if feame is potential candidate for loop closure with
// chosen distance type
- switch (params_.distance_type_)
+ switch (params_NNF->distance_type_)
{
case LoopclosureDistanceType::LC_POINT_ELLIPSE:
{
@@ -298,7 +298,7 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr&
bool ProcessorFrameNearestNeighborFilter::ellipse2DIntersect(const Eigen::VectorXs& ellipse1,
const Eigen::VectorXs& ellipse2)
{
- for (int i = 0 ; i < 360 ; i += params_.sample_step_degree_)
+ for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
{
// parameterized form of first ellipse gives point on first ellipse
// https://www.uwgb.edu/dutchs/Geometry/HTMLCanvas/ObliqueEllipses5.HTM
@@ -357,10 +357,10 @@ bool ProcessorFrameNearestNeighborFilter::ellipsoid3DIntersect(const Eigen::Vect
Eigen::Vector4s point_hom = Eigen::Vector4s::Constant(1);
Scalar omega = 0;
- for (int i = 0 ; i < 360 ; i += params_.sample_step_degree_)
+ for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
{
const Scalar theta = Scalar(i) * M_PI / 180.0;
- for( int j = 0 ; j < 180 ; j += params_.sample_step_degree_)
+ for( int j = 0 ; j < 180 ; j += params_NNF->sample_step_degree_)
{
omega = Scalar(j) * M_PI / 180.0;
@@ -483,7 +483,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
{
FrameBasePtr keyframe = getProblem()->getLastKeyFramePtr();
- if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_.buffer_size_ ))
+ if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
return false;
else
return true;
diff --git a/src/processor_frame_nearest_neighbor_filter.h b/src/processor_frame_nearest_neighbor_filter.h
index b4eb2e7fb79cfde1325dbb8bc5ca764e1095a2e3..1d9fb56b6db4ac05462699b35a4ff80eeeef3f52 100644
--- a/src/processor_frame_nearest_neighbor_filter.h
+++ b/src/processor_frame_nearest_neighbor_filter.h
@@ -55,7 +55,7 @@ private:
// depends on how many percent of data should be considered.
Scalar area_;
- ProcessorParamsFrameNearestNeighborFilter params_;
+ ProcessorParamsFrameNearestNeighborFilterPtr params_NNF;
public:
@@ -63,11 +63,11 @@ public:
using ParamsPtr = ProcessorParamsFrameNearestNeighborFilterPtr;
using DistanceType = Params::DistanceType;
- ProcessorFrameNearestNeighborFilter(const Params& _params);
+ ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF);
virtual ~ProcessorFrameNearestNeighborFilter() = default;
virtual void configure(SensorBasePtr _sensor) { };
- inline DistanceType getDistanceType() const noexcept {return params_.distance_type_;}
+ inline DistanceType getDistanceType() const noexcept {return params_NNF->distance_type_;}
protected:
diff --git a/src/processor_loopclosure_base.cpp b/src/processor_loopclosure_base.cpp
index ffb3b1c8e97d56f27b5d5b016e24169acf4ce332..160185238b25ecff2d63f8103a968a0d1eeefad4 100644
--- a/src/processor_loopclosure_base.cpp
+++ b/src/processor_loopclosure_base.cpp
@@ -10,8 +10,9 @@
namespace wolf
{
-ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, const Scalar _time_tolerance):
- ProcessorBase(_type, _time_tolerance)
+ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
+ ProcessorBase(_type, _params_loop_closure),
+ params_loop_closure_(_params_loop_closure)
{
//
}
diff --git a/src/processor_loopclosure_base.h b/src/processor_loopclosure_base.h
index a7e07e1114c33c6b42771ed60014822e4deeeec1..78dd9afce1c87054283410e4392c78ef6602e108 100644
--- a/src/processor_loopclosure_base.h
+++ b/src/processor_loopclosure_base.h
@@ -6,6 +6,8 @@
namespace wolf{
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
+
struct ProcessorParamsLoopClosure : public ProcessorParamsBase
{
// virtual ~ProcessorParamsLoopClosure() = default;
@@ -33,6 +35,9 @@ class ProcessorLoopClosureBase : public ProcessorBase
{
protected:
+
+ ProcessorParamsLoopClosurePtr params_loop_closure_;
+
// Frames that are possible loop closure candidates according to
// findLoopClosure()
std::vector<FrameBasePtr> loop_closure_candidates;
@@ -45,7 +50,7 @@ protected:
public:
- ProcessorLoopClosureBase(const std::string& _type, const Scalar _time_tolerance);
+ ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
virtual ~ProcessorLoopClosureBase() = default;
virtual void configure(SensorBasePtr _sensor) override { };
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index de044f52334c94b49688df4f2aa3d2c6150f0b3a..a4a00586e9ca0aa4abab2ec366ceac764cffa39a 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -3,13 +3,14 @@ namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
- Scalar _time_tolerance,
Size _state_size,
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Size _calib_size) :
- ProcessorBase(_type, _time_tolerance),
+ Size _calib_size,
+ ProcessorParamsMotionPtr _params_motion) :
+ ProcessorBase(_type, _params_motion),
+ params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_PACK),
x_size_(_state_size),
data_size_(_data_size),
@@ -244,7 +245,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
last_ptr_ = new_capture_ptr;
// callback to other processors
- getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
+ getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
}
resetDerived(); // TODO see where to put this
@@ -582,11 +583,11 @@ KFPackPtr ProcessorMotion::computeProcessingStep()
throw std::runtime_error("ProcessorMotion received data before being initialized.");
}
- KFPackPtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, time_tolerance_);
+ KFPackPtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, params_motion_->time_tolerance);
if (pack)
{
- if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), time_tolerance_))
+ if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
@@ -594,7 +595,7 @@ KFPackPtr ProcessorMotion::computeProcessingStep()
// throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
}
- else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp() - time_tolerance_)
+ else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp() - params_motion_->time_tolerance)
processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
else
diff --git a/src/processor_motion.h b/src/processor_motion.h
index df3471855ac4b030db679cfa7cd0027afc9328b9..a2706c4456fc411750ff89909e880cdffff10731 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -18,7 +18,17 @@
namespace wolf
{
-
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
+
+struct ProcessorParamsMotion : public ProcessorParamsBase
+{
+ Scalar max_time_span = 0.5;
+ Size max_buff_length = 10;
+ Scalar dist_traveled = 5;
+ Scalar angle_turned = 0.5;
+};
+
/** \brief class for Motion processors
*
* This processor integrates motion data into vehicle states.
@@ -108,17 +118,18 @@ class ProcessorMotion : public ProcessorBase
} ProcessingStep ;
protected:
+ ProcessorParamsMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
- Scalar _time_tolerance,
Size _state_size,
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Size _calib_size = 0);
+ Size _calib_size,
+ ProcessorParamsMotionPtr _params_motion);
virtual ~ProcessorMotion();
// Instructions to the processor:
@@ -400,10 +411,22 @@ class ProcessorMotion : public ProcessorBase
bool hasCalibration() {return calib_size_ > 0;}
public:
+ //getters
CaptureMotionPtr getOriginPtr();
CaptureMotionPtr getLastPtr();
CaptureMotionPtr getIncomingPtr();
+ Scalar getMaxTimeSpan() const;
+ Scalar getMaxBuffLength() const;
+ Scalar getDistTraveled() const;
+ Scalar getAngleTurned() const;
+
+ void setMaxTimeSpan(const Scalar& _max_time_span);
+ void setMaxBuffLength(const Scalar& _max_buff_length);
+ void setDistTraveled(const Scalar& _dist_traveled);
+ void setAngleTurned(const Scalar& _angle_turned);
+
+
protected:
// Attributes
@@ -451,7 +474,6 @@ inline void ProcessorMotion::resetDerived()
inline bool ProcessorMotion::voteForKeyFrame()
{
-
return false;
}
@@ -549,6 +571,44 @@ inline CaptureMotionPtr ProcessorMotion::getIncomingPtr()
return incoming_ptr_;
}
+inline Scalar ProcessorMotion::getMaxTimeSpan() const
+{
+ return params_motion_->max_time_span;
+}
+
+inline Scalar ProcessorMotion::getMaxBuffLength() const
+{
+ return params_motion_->max_buff_length;
+}
+
+inline Scalar ProcessorMotion::getDistTraveled() const
+{
+ return params_motion_->dist_traveled;
+}
+
+inline Scalar ProcessorMotion::getAngleTurned() const
+{
+ return params_motion_->angle_turned;
+}
+
+inline void ProcessorMotion::setMaxTimeSpan(const Scalar& _max_time_span)
+{
+ params_motion_->max_time_span = _max_time_span;
+}
+inline void ProcessorMotion::setMaxBuffLength(const Scalar& _max_buff_length)
+{
+ params_motion_->max_buff_length = _max_buff_length;
+}
+inline void ProcessorMotion::setDistTraveled(const Scalar& _dist_traveled)
+{
+ params_motion_->dist_traveled = _dist_traveled;
+}
+inline void ProcessorMotion::setAngleTurned(const Scalar& _angle_turned)
+{
+ params_motion_->angle_turned = _angle_turned;
+}
+
+
} // namespace wolf
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index 5ebc1bffafb2937cfb3e017e57df9af70274e4e5..5f931f44377d422bea9c416f448ef1639ac2545f 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -2,14 +2,15 @@
namespace wolf
{
-ProcessorOdom2D::ProcessorOdom2D(const ProcessorParamsOdom2D& _params) :
- ProcessorMotion("ODOM 2D", _params.time_tolerance, 3, 3, 3, 2, 0),
- dist_traveled_th_(_params.dist_traveled_th_),
- theta_traveled_th_(_params.theta_traveled_th_),
- cov_det_th_(_params.cov_det_th_),
- elapsed_time_th_(_params.elapsed_time_th_)
+ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
+ ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params),
+ params_odom_2D_(_params)
+// dist_traveled_th_(_params.dist_traveled_th_),
+// theta_traveled_th_(_params.theta_traveled_th_),
+// cov_det_th_(_params->cov_det_th)//,
+// elapsed_time_th_(_params.elapsed_time_th_)
{
- unmeasured_perturbation_cov_ = _params.unmeasured_perturbation_std_ * _params.unmeasured_perturbation_std_ * Matrix3s::Identity();
+ unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity();
}
ProcessorOdom2D::~ProcessorOdom2D()
@@ -121,21 +122,22 @@ Motion ProcessorOdom2D::interpolate(const Motion& _ref, Motion& _second, TimeSta
bool ProcessorOdom2D::voteForKeyFrame()
{
// Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > dist_traveled_th_)
+ if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2D_->dist_traveled)
{
return true;
}
- if (getBuffer().get().back().delta_integr_.tail<1>().norm() > theta_traveled_th_)
+ if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2D_->angle_turned)
{
return true;
}
// Uncertainty criterion
- if (getBuffer().get().back().delta_integr_cov_.determinant() > cov_det_th_)
+ if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2D_->cov_det)
{
return true;
}
// Time criterion
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get() > elapsed_time_th_)
+ 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)
{
return true;
}
@@ -172,19 +174,13 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const
ProcessorOdom2DPtr prc_ptr;
+ std::shared_ptr<ProcessorParamsOdom2D> params;
if (_params)
- {
- std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
-
- prc_ptr = std::make_shared<ProcessorOdom2D>(*params);
- }
+ params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
else
- {
- std::cout << __FILE__ << ":" << __FUNCTION__ << "() : No parameters provided. Using default set." << std::endl;
-
- prc_ptr = std::make_shared<ProcessorOdom2D>();
- }
+ params = std::make_shared<ProcessorParamsOdom2D>();
+ prc_ptr = std::make_shared<ProcessorOdom2D>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h
index c877826af4585d64d530414f451f2d010c347518..bf797ca8b24f488234c1f7eb5a3c7cf3f317e46c 100644
--- a/src/processor_odom_2D.h
+++ b/src/processor_odom_2D.h
@@ -19,19 +19,16 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorOdom2D);
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
-struct ProcessorParamsOdom2D : public ProcessorParamsBase
+struct ProcessorParamsOdom2D : public ProcessorParamsMotion
{
- Scalar dist_traveled_th_ = 1.0; // 1m
- Scalar theta_traveled_th_ = 0.17; // 90 degrees
- Scalar cov_det_th_ = 1.0; // 1 rad^2
- Scalar elapsed_time_th_ = 1.0; // 1s
- Scalar unmeasured_perturbation_std_ = 0.001; // no particular dimension: the same for displacement and angle
+ Scalar cov_det = 1.0; // 1 rad^2
+ Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle
};
class ProcessorOdom2D : public ProcessorMotion
{
public:
- ProcessorOdom2D(const ProcessorParamsOdom2D& _params = ProcessorParamsOdom2D());
+ ProcessorOdom2D(ProcessorParamsOdom2DPtr _params);
virtual ~ProcessorOdom2D();
virtual void configure(SensorBasePtr _sensor) override { };
@@ -74,11 +71,8 @@ class ProcessorOdom2D : public ProcessorMotion
CaptureBasePtr _capture_origin) override;
protected:
- Scalar dist_traveled_th_;
- Scalar theta_traveled_th_;
- Scalar cov_det_th_;
- Scalar elapsed_time_th_;
- Matrix3s unmeasured_perturbation_cov_; ///< Covariance to be added to the unmeasured perturbation
+ ProcessorParamsOdom2DPtr params_odom_2D_;
+ MatrixXs unmeasured_perturbation_cov_;
// Factory method
public:
diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index 6a875eebb5daf3a4b713038ad5383f7a7cce6e02..49da6be8103ab17a3a50ed75dd87492db3dedb40 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -2,12 +2,9 @@
namespace wolf
{
-ProcessorOdom3D::ProcessorOdom3D(const ProcessorParamsOdom3D& _params, SensorOdom3DPtr _sensor_ptr) :
- ProcessorMotion("ODOM 3D", _params.time_tolerance, 7, 7, 6, 6, 0 ),
- max_time_span_ ( _params.max_time_span ),
- max_buff_length_( _params.max_buff_length ),
- dist_traveled_ ( _params.dist_traveled ),
- angle_turned_ ( _params.angle_turned ),
+ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr) :
+ ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params),
+ params_odom_3D_(_params),
p1_(nullptr), p2_(nullptr), p_out_(nullptr),
q1_(nullptr), q2_(nullptr), q_out_(nullptr)
{
@@ -267,7 +264,7 @@ ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const
SensorOdom3DPtr sen_odo =std::static_pointer_cast<SensorOdom3D>(_sen_ptr);
// construct processor
- ProcessorOdom3DPtr prc_odo = std::make_shared<ProcessorOdom3D>(*prc_odo_params, sen_odo);
+ ProcessorOdom3DPtr prc_odo = std::make_shared<ProcessorOdom3D>(prc_odo_params, sen_odo);
// setup processor
prc_odo->setName(_unique_name);
@@ -284,27 +281,27 @@ bool ProcessorOdom3D::voteForKeyFrame()
//WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
//WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
// time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > max_time_span_)
+ if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3D_->max_time_span)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().get().size() > max_buff_length_)
+ if (getBuffer().get().size() > params_odom_3D_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer size" );
return true;
}
// distance traveled
Scalar dist = getMotion().delta_integr_.head(3).norm();
- if (dist > dist_traveled_)
+ if (dist > params_odom_3D_->dist_traveled)
{
WOLF_DEBUG( "PM: vote: distance traveled" );
return true;
}
// angle turned
Scalar angle = q2v(Quaternions(getMotion().delta_integr_.data()+3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
- if (angle > angle_turned_)
+ if (angle > params_odom_3D_->angle_turned)
{
WOLF_DEBUG( "PM: vote: angle turned" );
return true;
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 67aee2680297d889eb11ec326ca212ada6c65e25..9a9828d4b2dcc8574de9b22ba8d55503e7a3875a 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -20,12 +20,9 @@ namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
-struct ProcessorParamsOdom3D : public ProcessorParamsBase
+struct ProcessorParamsOdom3D : public ProcessorParamsMotion
{
- Scalar max_time_span = 1.0;
- Size max_buff_length = 10;
- Scalar dist_traveled = 1.0;
- Scalar angle_turned = 0.5;
+ //
};
@@ -56,7 +53,7 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
class ProcessorOdom3D : public ProcessorMotion
{
public:
- ProcessorOdom3D(const ProcessorParamsOdom3D& _params = ProcessorParamsOdom3D(), SensorOdom3DPtr _sensor_ptr = nullptr);
+ ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr = nullptr);
virtual ~ProcessorOdom3D();
virtual void configure(SensorBasePtr _sensor) override;
@@ -97,6 +94,8 @@ class ProcessorOdom3D : public ProcessorMotion
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
protected:
+ ProcessorParamsOdom3DPtr params_odom_3D_;
+
// noise parameters (stolen from owner SensorOdom3D)
Scalar k_disp_to_disp_; // displacement variance growth per meter of linear motion
Scalar k_disp_to_rot_; // orientation variance growth per meter of linear motion
@@ -104,12 +103,6 @@ class ProcessorOdom3D : public ProcessorMotion
Scalar min_disp_var_; // floor displacement variance when no motion
Scalar min_rot_var_; // floor orientation variance when no motion
- // keyframe voting parameters
- Scalar max_time_span_; // maximum time between keyframes
- Size max_buff_length_;// maximum buffer size before keyframe
- Scalar dist_traveled_; // maximum linear motion between keyframes
- Scalar angle_turned_; // maximum rotation between keyframes
-
// Eigen::Map helpers
Eigen::Map<const Eigen::Vector3s> p1_, p2_;
Eigen::Map<Eigen::Vector3s> p_out_;
@@ -122,26 +115,6 @@ class ProcessorOdom3D : public ProcessorMotion
static ProcessorBasePtr create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr sensor_ptr = nullptr);
-
- void setAngleTurned(Scalar angleTurned)
- {
- angle_turned_ = angleTurned;
- }
-
- void setDistTraveled(Scalar distTraveled)
- {
- dist_traveled_ = distTraveled;
- }
-
- void setMaxBuffLength(Size maxBuffLength)
- {
- max_buff_length_ = maxBuffLength;
- }
-
- void setMaxTimeSpan(Scalar maxTimeSpan)
- {
- max_time_span_ = maxTimeSpan;
- }
};
inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
diff --git a/src/processor_params_image.h b/src/processor_params_image.h
index 793f2566f16c0851072e5974a65097320255c265..fde468176228453f2bd25551c6beb87617b29b67 100644
--- a/src/processor_params_image.h
+++ b/src/processor_params_image.h
@@ -2,36 +2,36 @@
#define PROCESSOR_IMAGE_PARAMS_H
// wolf
-#include "processor_base.h"
-#include "wolf.h"
-
-// Vision utils
-#include <vision_utils.h>
-#include <vision_utils/detectors.h>
-#include <vision_utils/descriptors.h>
-#include <vision_utils/matchers.h>
+#include "processor_tracker_feature.h"
+#include "processor_tracker_landmark.h"
namespace wolf
{
-struct ProcessorParamsImage : public ProcessorParamsBase
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureImage);
+
+struct ProcessorParamsTrackerFeatureImage : public ProcessorParamsTrackerFeature
+{
+ std::string yaml_file_params_vision_utils;
+
+ Scalar min_response_for_new_features; ///< minimum value of the response to create a new feature
+ Scalar distance;
+
+ Scalar pixel_noise_std; ///< std noise of the pixel
+ Scalar pixel_noise_var; ///< var noise of the pixel
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkImage);
+
+struct ProcessorParamsTrackerLandmarkImage : public ProcessorParamsTrackerLandmark
{
- std::string yaml_file_params_vision_utils;
-
- struct Algorithm
- {
- unsigned int max_new_features; ///< Max nbr. of features to detect in one frame
- unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
- float min_response_for_new_features; ///< minimum value of the response to create a new feature
- Scalar time_tolerance;
- Scalar distance;
- }algorithm;
-
- struct Noise
- {
- Scalar pixel_noise_std; ///< std noise of the pixel
- Scalar pixel_noise_var; ///< var noise of the pixel
- }noise;
+ std::string yaml_file_params_vision_utils;
+
+ Scalar min_response_for_new_features; ///< minimum value of the response to create a new feature
+ Scalar distance;
+
+ Scalar pixel_noise_std; ///< std noise of the pixel
+ Scalar pixel_noise_var; ///< var noise of the pixel
};
}
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index 741eacb07aed84a327e3fe38c72210819a4d2486..c71c7f9d9d835b73e83e00880b653f7d50dcea45 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -14,13 +14,15 @@
namespace wolf
{
-ProcessorTracker::ProcessorTracker(const std::string& _type, const Scalar& _time_tolerance, const unsigned int _max_new_features) :
- ProcessorBase(_type, _time_tolerance),
+ProcessorTracker::ProcessorTracker(const std::string& _type,
+ ProcessorParamsTrackerPtr _params_tracker) :
+ ProcessorBase(_type, _params_tracker),
+ params_tracker_(_params_tracker),
processing_step_(FIRST_TIME_WITHOUT_PACK),
origin_ptr_(nullptr),
last_ptr_(nullptr),
incoming_ptr_(nullptr),
- max_new_features_(_max_new_features)
+ number_of_tracks_(0)
{
//
}
@@ -50,7 +52,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
case FIRST_TIME_WITH_PACK :
{
- KFPackPtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, time_tolerance_);
+ KFPackPtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -78,7 +80,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We only process new features in Last, here last = nullptr, so we do not have anything to do.
// Issue KF callback with new KF
- getProblem()->keyFrameCallback(kfrm, shared_from_this(), time_tolerance_);
+ getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
// Update pointers
resetDerived();
@@ -90,7 +92,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case SECOND_TIME_WITH_PACK :
{
- KFPackPtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, time_tolerance_);
+ // No-break case only for debug. Next case will be executed too.
+ KFPackPtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
}
case SECOND_TIME_WITHOUT_PACK :
@@ -101,7 +104,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
- processNew(max_new_features_);
+ processNew(params_tracker_->max_new_features);
// Update pointers
resetDerived();
@@ -113,7 +116,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case RUNNING_WITH_PACK :
{
- KFPackPtr pack = kf_pack_buffer_.selectPack( last_ptr_ , time_tolerance_);
+ KFPackPtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -131,7 +134,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
frm->addCapture(incoming_ptr_);
// Detect new Features, initialize Landmarks, create Constraints, ...
- processNew(max_new_features_);
+ processNew(params_tracker_->max_new_features);
// Establish constraints
establishConstraints();
@@ -148,6 +151,13 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
processKnown();
+ // eventually add more features
+ if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe)
+ {
+ WOLF_TRACE("Adding more features...");
+ processNew(params_tracker_->max_new_features);
+ }
+
if (voteForKeyFrame() && permittedKeyFrame())
{
// We create a KF
@@ -160,11 +170,11 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
frm->addCapture(incoming_ptr_);
// process
- processNew(max_new_features_);
-
- // Set key
- last_ptr_->getFramePtr()->setKey();
+// processNew(params_tracker_->max_new_features);
+ // // Set key
+ // last_ptr_->getFramePtr()->setKey();
+ //
// Set state to the keyframe
last_ptr_->getFramePtr()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
@@ -172,7 +182,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
establishConstraints();
// Call the new keyframe callback in order to let the other processors to establish their constraints
- getProblem()->keyFrameCallback(last_ptr_->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), time_tolerance_);
+ getProblem()->keyFrameCallback(last_ptr_->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
// Update pointers
resetDerived();
@@ -201,6 +211,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
break;
}
+ number_of_tracks_ = last_ptr_->getFeatureList().size();
postProcess();
}
@@ -221,7 +232,7 @@ void ProcessorTracker::computeProcessingStep()
{
case FIRST_TIME :
- if (kf_pack_buffer_.selectPack(incoming_ptr_, time_tolerance_))
+ if (kf_pack_buffer_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
processing_step_ = FIRST_TIME_WITH_PACK;
else // ! last && ! pack(incoming)
processing_step_ = FIRST_TIME_WITHOUT_PACK;
@@ -229,7 +240,7 @@ void ProcessorTracker::computeProcessingStep()
case SECOND_TIME :
- if (kf_pack_buffer_.selectPack(last_ptr_, time_tolerance_))
+ if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
processing_step_ = SECOND_TIME_WITH_PACK;
else
processing_step_ = SECOND_TIME_WITHOUT_PACK;
@@ -238,7 +249,7 @@ void ProcessorTracker::computeProcessingStep()
case RUNNING :
default :
- if (kf_pack_buffer_.selectPack(last_ptr_, time_tolerance_))
+ if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
{
if (last_ptr_->getFramePtr()->isKey())
{
diff --git a/src/processor_tracker.h b/src/processor_tracker.h
index 2ed1ae6e4624af8bb359a9507598e92b58301850..957ae5a6776bdeffba152279d4aa58918019f099 100644
--- a/src/processor_tracker.h
+++ b/src/processor_tracker.h
@@ -13,13 +13,16 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ProcessorTracker);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
struct ProcessorParamsTracker : public ProcessorParamsBase
{
+ unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
unsigned int max_new_features;
};
+WOLF_PTR_TYPEDEFS(ProcessorTracker);
+
/** \brief General tracker processor
*
* This is an abstract class.
@@ -78,16 +81,19 @@ class ProcessorTracker : public ProcessorBase
} ProcessingStep ;
protected:
+ ProcessorParamsTrackerPtr params_tracker_; ///< parameters for the tracker
ProcessingStep processing_step_; ///< State machine controlling the processing step
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- unsigned int max_new_features_; ///< max features allowed to detect in one iteration. 0 = no limit
FeatureBaseList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
FeatureBaseList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
+ size_t number_of_tracks_;
+
public:
- ProcessorTracker(const std::string& _type, const Scalar& _time_tolerance, const unsigned int _max_new_features);
+ ProcessorTracker(const std::string& _type,
+ ProcessorParamsTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
/** \brief Full processing of an incoming Capture.
@@ -97,9 +103,6 @@ class ProcessorTracker : public ProcessorBase
*/
virtual void process(CaptureBasePtr const _incoming_ptr);
- void setMaxNewFeatures(const unsigned int& _max_new_features);
- unsigned int getMaxNewFeatures();
-
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
@@ -193,6 +196,16 @@ class ProcessorTracker : public ProcessorBase
FeatureBaseList& getNewFeaturesListLast();
+ const size_t& previousNumberOfTracks() const
+ {
+ return number_of_tracks_;
+ }
+
+ size_t& previousNumberOfTracks()
+ {
+ return number_of_tracks_;
+ }
+
protected:
void computeProcessingStep();
@@ -205,16 +218,6 @@ class ProcessorTracker : public ProcessorBase
};
-inline void ProcessorTracker::setMaxNewFeatures(const unsigned int& _max_new_features)
-{
- max_new_features_ = _max_new_features;
-}
-
-inline unsigned int ProcessorTracker::getMaxNewFeatures()
-{
- return max_new_features_;
-}
-
inline FeatureBaseList& ProcessorTracker::getNewFeaturesListLast()
{
return new_features_last_;
@@ -232,7 +235,7 @@ inline FeatureBaseList& ProcessorTracker::getNewFeaturesListIncoming()
inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
{
- return (std::fabs(_ts2 - _ts2) < time_tolerance_);
+ return (std::fabs(_ts2 - _ts2) < params_tracker_->time_tolerance);
}
inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index ad4382dbc1a27c4a9a630b866b3d7a241aab98ae..f6dee2871290f188bf3695321e205d03e9f2181d 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -10,8 +10,10 @@
namespace wolf
{
-ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, const Scalar _time_tolerance, const unsigned int _max_new_features) :
- ProcessorTracker(_type, _time_tolerance, _max_new_features)
+ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
+ ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
+ ProcessorTracker(_type, _params_tracker_feature),
+ params_tracker_feature_(_params_tracker_feature)
{
}
@@ -37,6 +39,7 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
for (auto ftr : new_features_incoming_)
{
+ ftr->setProblem(this->getProblem());
size_t trk_id_from_last = matches_last_from_incoming_[ftr]->feature_ptr_->trackId();
track_matrix_.add(trk_id_from_last, incoming_ptr_, ftr);
}
@@ -74,7 +77,11 @@ unsigned int ProcessorTrackerFeature::processKnown()
size_t track_id = feature_in_incoming->trackId();
FeatureBasePtr feature_in_last = track_matrix_.feature(track_id, last_ptr_);
FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_);
- if (!(correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming)))
+ if (correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
+ {
+ feature_in_incoming->setProblem(this->getProblem());
+ }
+ else
{
// Remove this feature from many places:
matches_last_from_incoming_ .erase (feature_in_incoming); // remove match
@@ -105,8 +112,8 @@ void ProcessorTrackerFeature::advanceDerived()
for (auto ftr : incoming_ptr_->getFeatureList())
ftr->setProblem(getProblem());
- // remove last from track matrix
- track_matrix_.remove(last_ptr_);
+ // // remove last from track matrix in case you want to have only KF in the track matrix
+ // track_matrix_.remove(last_ptr_);
}
void ProcessorTrackerFeature::resetDerived()
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 6b3b5f467a379ab019c3e08eb4f9c4a640167860..e51bd731282fa81c25d059ee23b8810041244ded 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -17,6 +17,13 @@
namespace wolf
{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeature);
+
+struct ProcessorParamsTrackerFeature : public ProcessorParamsTracker
+{
+ //
+};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
@@ -63,7 +70,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
* - establishConstraints() : which calls the pure virtual:
* - createConstraint() : create constraint of the correct derived type <=== IMPLEMENT
*
- * Should you need extra functionality for your derived types, you can overload these two methods,
+ * Should you need extra functionality for your derived types, you can override these two methods,
*
* - preProcess() { }
* - postProcess() { }
@@ -77,10 +84,12 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Constructor with type
*/
- ProcessorTrackerFeature(const std::string& _type, const Scalar _time_tolerance, const unsigned int _max_new_features);
+ ProcessorTrackerFeature(const std::string& _type,
+ ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
virtual ~ProcessorTrackerFeature();
protected:
+ ProcessorParamsTrackerFeaturePtr params_tracker_feature_;
TrackMatrix track_matrix_;
FeatureBaseList known_features_incoming_;
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 262d8cff26f19eb8901360a851c0ada831e0155a..3b0ac96a266126ed620796ec3e9acfe7b3a2658f 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -11,13 +11,9 @@
namespace wolf
{
-ProcessorTrackerFeatureCorner::ProcessorTrackerFeatureCorner(
- const laserscanutils::LineFinderIterativeParams& _line_finder_params,
- const Scalar& _time_tolerance,
- const unsigned int& _n_corners_th) :
- ProcessorTrackerFeature("TRACKER FEATURE CORNER", _time_tolerance, 0),
- line_finder_(_line_finder_params),
- n_tracks_th_(_n_corners_th),
+ProcessorTrackerFeatureCorner::ProcessorTrackerFeatureCorner(ProcessorParamsTrackerFeatureCornerPtr _params_tracker_feature_corner) :
+ ProcessorTrackerFeature("TRACKER FEATURE CORNER", _params_tracker_feature_corner),
+ params_tracker_feature_corner_(_params_tracker_feature_corner),
R_world_sensor_(Eigen::Matrix3s::Identity()),
R_robot_sensor_(Eigen::Matrix3s::Identity()),
extrinsics_transformation_computed_(false)
@@ -89,7 +85,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
auto feat_out_it = feat_out_next++; // next is used to obtain the next iterator after splice
while (feat_out_it != corners_incoming_.end()) //runs over extracted feature
{
- if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > position_error_th_*position_error_th_)
+ if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > params_tracker_feature_corner_->position_error_th*params_tracker_feature_corner_->position_error_th)
{
// match
_feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
@@ -108,7 +104,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
{
- return incoming_ptr_->getFeatureList().size() < n_tracks_th_;
+ return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
}
unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
@@ -146,13 +142,16 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr
}
void ProcessorTrackerFeatureCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBaseList& _corner_list)
+ FeatureBaseList& _corner_list)
{
// TODO: sort corners by bearing
std::list<laserscanutils::CornerPoint> corners;
std::cout << "Extracting corners..." << std::endl;
- 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>(getSensorPtr()))->getScanParams(),
+ line_finder_,
+ corners);
Eigen::Vector4s measurement;
for (auto corner : corners)
diff --git a/src/processor_tracker_feature_corner.h b/src/processor_tracker_feature_corner.h
index 8c0d23eacc4b1a91dc13980cc1204156798d5f80..93c4c7add4e1e2b7e6cc1b022dffa92a7a5370ea 100644
--- a/src/processor_tracker_feature_corner.h
+++ b/src/processor_tracker_feature_corner.h
@@ -28,19 +28,28 @@
namespace wolf
{
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureCorner);
+
+struct ProcessorParamsTrackerFeatureCorner : public ProcessorParamsTrackerFeature
+{
+ laserscanutils::LineFinderIterativeParams line_finder_params;
+ unsigned int n_corners_th;
+ const Scalar position_error_th = 1;
+};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureCorner);
//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
+//const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
+//const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
+//const Scalar position_error_th_ = 1;
+//const Scalar min_features_ratio_th_ = 0.5;
class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
{
private:
+ ProcessorParamsTrackerFeatureCornerPtr params_tracker_feature_corner_;
//laserscanutils::ScanParams scan_params_;
//laserscanutils::ExtractCornerParams corner_alg_params_;
//laserscanutils::LaserScan laser_data_;
@@ -50,7 +59,6 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
FeatureBaseList corners_incoming_;
FeatureBaseList corners_last_;
- unsigned int n_tracks_th_;
Eigen::Matrix3s R_world_sensor_, R_world_sensor_prev_;
Eigen::Matrix3s R_robot_sensor_;
@@ -64,9 +72,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- ProcessorTrackerFeatureCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
- const Scalar& _time_tolerance,
- const unsigned int& _n_corners_th);
+ ProcessorTrackerFeatureCorner(ProcessorParamsTrackerFeatureCornerPtr _params_tracker_feature_corner);
virtual ~ProcessorTrackerFeatureCorner();
virtual void configure(SensorBasePtr _sensor) { };
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index e50549982884b2884fe6d423f49f3cd7d62669f4..b0679cf6ac236e21684933c3601ea7e2ca574fe5 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -42,11 +42,11 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
{
WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
- bool vote = incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
+ bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
- return incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
+ return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
}
unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index fdfbdd9b9d3342d9561351b3b441804bbdba9f85..09ae7df58dc6b2ca628086a952ace56d72ae7702 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/src/processor_tracker_feature_dummy.h
@@ -22,17 +22,14 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
{
public:
- ProcessorTrackerFeatureDummy(const Scalar _time_tolerance, const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe);
+ ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy();
virtual void configure(SensorBasePtr _sensor) { };
protected:
static unsigned int count_;
- unsigned int n_feature_, min_feat_for_keyframe_;
-
-// virtual void preProcess() { }
-// virtual void postProcess() { }
+ unsigned int n_feature_;
/** \brief Track provided features from \b last to \b incoming
* \param _feature_list_in input list of features in \b last to track
@@ -74,9 +71,9 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
};
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const Scalar _time_tolerance, const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe) :
- ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _time_tolerance, _max_new_features),
- n_feature_(0), min_feat_for_keyframe_(_min_feat_for_keyframe)
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
+ ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature),
+ n_feature_(0)
{
//
}
diff --git a/src/processor_tracker_feature_image.cpp b/src/processor_tracker_feature_image.cpp
index 18f54e0f96a0a8f286b7ca99b80a8713abba5f82..397a39cf6e3cbf111dbecabc5041e047ae0637cb 100644
--- a/src/processor_tracker_feature_image.cpp
+++ b/src/processor_tracker_feature_image.cpp
@@ -2,10 +2,10 @@
#include "processor_tracker_feature_image.h"
// Vision utils
-#include <vision_utils/detectors.h>
-#include <vision_utils/descriptors.h>
-#include <vision_utils/matchers.h>
-#include <vision_utils/algorithms.h>
+#include <detectors.h>
+#include <descriptors.h>
+#include <matchers.h>
+#include <algorithms.h>
// standard libs
#include <bitset>
@@ -14,14 +14,14 @@
namespace wolf
{
-ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsImage _params) :
- ProcessorTrackerFeature("IMAGE", _params.time_tolerance, _params.algorithm.max_new_features),
+ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsTrackerFeatureImagePtr _params_tracker_feature_image) :
+ ProcessorTrackerFeature("IMAGE", _params_tracker_feature_image),
cell_width_(0), cell_height_(0), // These will be initialized to proper values taken from the sensor via function configure()
- params_(_params)
+ params_tracker_feature_image_(_params_tracker_feature_image)
{
// Detector
- std::string det_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_.yaml_file_params_vision_utils);
+ std::string det_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "detector");
+ det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_feature_image_->yaml_file_params_vision_utils);
if (det_name.compare("ORB") == 0)
det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
@@ -49,8 +49,8 @@ ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsImage
det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
// Descriptor
- std::string des_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_.yaml_file_params_vision_utils);
+ std::string des_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "descriptor");
+ des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_feature_image_->yaml_file_params_vision_utils);
if (des_name.compare("ORB") == 0)
des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
@@ -76,8 +76,8 @@ ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsImage
des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
// Matcher
- std::string mat_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_.yaml_file_params_vision_utils);
+ std::string mat_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "matcher");
+ mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_image_->yaml_file_params_vision_utils);
if (mat_name.compare("FLANNBASED") == 0)
mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
@@ -91,7 +91,7 @@ ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsImage
mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
// Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_.yaml_file_params_vision_utils);
+ vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_tracker_feature_image_->yaml_file_params_vision_utils);
active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
}
@@ -110,10 +110,10 @@ void ProcessorTrackerFeatureImage::configure(SensorBasePtr _sensor)
active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight() , det_ptr_->getPatternRadius());
- params_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
+ params_tracker_feature_image_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
- cell_width_ = image_.width_ / params_activesearch_ptr_->n_cells_h;
- cell_height_ = image_.height_ / params_activesearch_ptr_->n_cells_v;
+ cell_width_ = image_.width_ / params_tracker_feature_image_activesearch_ptr_->n_cells_h;
+ cell_height_ = image_.height_ / params_tracker_feature_image_activesearch_ptr_->n_cells_v;
}
void ProcessorTrackerFeatureImage::preProcess()
@@ -162,8 +162,10 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList&
if ( normalized_score > mat_ptr_->getParams()->min_norm_score )
{
FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx], (candidate_descriptors.row(cv_matches[0].trainIdx)),
- Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var);
+ candidate_keypoints[cv_matches[0].trainIdx],
+ cv_matches[0].trainIdx,
+ (candidate_descriptors.row(cv_matches[0].trainIdx)),
+ Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
_feature_list_out.push_back(incoming_point_ptr);
@@ -266,13 +268,14 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
if(list_keypoints[i].pt == new_keypoints[0].pt)
index = i;
}
- if(new_keypoints[0].response > params_activesearch_ptr_->min_response_new_feature)
+ if(new_keypoints[0].response > params_tracker_feature_image_activesearch_ptr_->min_response_new_feature)
{
std::cout << "response: " << new_keypoints[0].response << std::endl;
FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
new_keypoints[0],
+ 0,
new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var);
+ Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
point_ptr->setIsKnown(false);
_feature_list_out.push_back(point_ptr);
@@ -298,7 +301,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
Scalar ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
{
- mat_ptr_->match(_target_descriptor, _candidate_descriptors, _cv_matches);
+ mat_ptr_->match(_target_descriptor, _candidate_descriptors, des_ptr_->getSize(), _cv_matches);
Scalar normalized_score = 1 - (Scalar)(_cv_matches[0].distance)/(des_ptr_->getSize()*8);
return normalized_score;
}
@@ -382,7 +385,7 @@ void ProcessorTrackerFeatureImage::drawFeatures(cv::Mat _image)
ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr)
{
- ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(*(std::static_pointer_cast<ProcessorParamsImage>(_params)));
+ ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(std::static_pointer_cast<ProcessorParamsTrackerFeatureImage>(_params));
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor_tracker_feature_image.h b/src/processor_tracker_feature_image.h
index 8f5c3e7ccd41e0871ede7c53e335a6e7d9528ec2..26cbaada43bc72ba89d3edb38fda48eb94eace66 100644
--- a/src/processor_tracker_feature_image.h
+++ b/src/processor_tracker_feature_image.h
@@ -11,11 +11,11 @@
#include "constraint_epipolar.h"
#include "processor_params_image.h"
-// Vision utils
-#include <vision_utils/detectors/detector_base.h>
-#include <vision_utils/descriptors/descriptor_base.h>
-#include <vision_utils/matchers/matcher_base.h>
-#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
+
+#include <detectors/detector_base.h>
+#include <descriptors/descriptor_base.h>
+#include <matchers/matcher_base.h>
+#include <algorithms/activesearch/alg_activesearch.h>
// General includes
#include <cmath>
@@ -29,6 +29,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureImage);
//class
class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
{
+ // vision utils params
protected:
vision_utils::DetectorBasePtr det_ptr_;
@@ -38,11 +39,11 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
int cell_width_; ///< Active search cell width
int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_activesearch_ptr_; ///< Active search parameters
+ vision_utils::AlgorithmParamsACTIVESEARCHPtr params_tracker_feature_image_activesearch_ptr_; ///< Active search parameters
protected:
- ProcessorParamsImage params_; ///< Struct with parameters of the processors
+ ProcessorParamsTrackerFeatureImagePtr params_tracker_feature_image_; ///< Struct with parameters of the processors
cv::Mat image_last_, image_incoming_; ///< Images of the "last" and "incoming" Captures
struct
@@ -59,7 +60,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
std::list<cv::Rect> detector_roi_;
std::list<cv::Point> tracker_target_;
- ProcessorTrackerFeatureImage(ProcessorParamsImage _params);
+ ProcessorTrackerFeatureImage(ProcessorParamsTrackerFeatureImagePtr _params_image);
virtual ~ProcessorTrackerFeatureImage();
virtual void configure(SensorBasePtr _sensor) ;
@@ -160,7 +161,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
inline bool ProcessorTrackerFeatureImage::voteForKeyFrame()
{
- return (incoming_ptr_->getFeatureList().size() < params_.algorithm.min_features_for_keyframe);
+ return (incoming_ptr_->getFeatureList().size() < params_tracker_feature_image_->min_features_for_keyframe);
}
} // namespace wolf
diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp
index 0098b621202dfe7f17654373ad9d3ad506ef5d70..e68101044926a208ea031fbf005421b62c1c26c8 100644
--- a/src/processor_tracker_landmark.cpp
+++ b/src/processor_tracker_landmark.cpp
@@ -14,9 +14,12 @@
namespace wolf
{
-ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, const Scalar& _time_tolerance, const unsigned int& _max_new_features) :
- ProcessorTracker(_type, _time_tolerance, _max_new_features)
+ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
+ ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
+ ProcessorTracker(_type, _params_tracker_landmark),
+ params_tracker_landmark_(_params_tracker_landmark)
{
+ //
}
ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index 7c7a3e0a28c0d580367d6550ff1a7a5d4767a040..479d0d8ea68622a2b4c924425c698534a37daf5d 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -16,6 +16,14 @@
namespace wolf
{
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmark);
+
+struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker
+{
+ //
+};
+
+
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
/** \brief Landmark tracker processor
@@ -72,11 +80,13 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
- ProcessorTrackerLandmark(const std::string& _type, const Scalar& _time_tolerance, const unsigned int& _max_new_features);
+ ProcessorTrackerLandmark(const std::string& _type,
+ ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
virtual ~ProcessorTrackerLandmark();
protected:
+ ProcessorParamsTrackerLandmarkPtr params_tracker_landmark_;
LandmarkBaseList new_landmarks_; ///< List of new detected landmarks
LandmarkMatchMap matches_landmark_from_incoming_;
LandmarkMatchMap matches_landmark_from_last_;
diff --git a/src/processor_tracker_landmark_corner.cpp b/src/processor_tracker_landmark_corner.cpp
index cbff09b2f8ef6cd26ad0def3a409f0fe498c70cc..c69b9964ed844402ba33f0781dfeeecb68ec7f9e 100644
--- a/src/processor_tracker_landmark_corner.cpp
+++ b/src/processor_tracker_landmark_corner.cpp
@@ -359,7 +359,7 @@ Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistanc
ProcessorBasePtr ProcessorTrackerLandmarkCorner::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
{
ProcessorParamsLaserPtr params = std::static_pointer_cast<ProcessorParamsLaser>(_params);
- ProcessorTrackerLandmarkCornerPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkCorner>(params->line_finder_params_, _params->time_tolerance, params->new_corners_th, params->loop_frames_th);
+ ProcessorTrackerLandmarkCornerPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkCorner>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
@@ -406,15 +406,11 @@ ProcessorTrackerLandmarkCorner::~ProcessorTrackerLandmarkCorner()
}
}
-ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(
- const laserscanutils::LineFinderIterativeParams& _line_finder_params,
- const Scalar& _time_tolerance,
- const unsigned int& _new_corners_th,
- const unsigned int& _loop_frames_th) :
- ProcessorTrackerLandmark("TRACKER LANDMARK CORNER", _time_tolerance, 0),
- line_finder_(_line_finder_params),
- new_corners_th_(_new_corners_th),
- loop_frames_th_(_loop_frames_th),
+ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(ProcessorParamsLaserPtr _params_laser) :
+ ProcessorTrackerLandmark("TRACKER LANDMARK CORNER", _params_laser),
+ line_finder_(_params_laser->line_finder_params_),
+ new_corners_th_(_params_laser->new_corners_th),
+ loop_frames_th_(_params_laser->loop_frames_th),
R_sensor_world_(Eigen::Matrix3s::Identity()),
R_world_sensor_(Eigen::Matrix3s::Identity()),
R_robot_sensor_(Eigen::Matrix3s::Identity()),
diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h
index 3c8ed3a39b282f6a6442c21063d6810c4d51dcf4..56619fcf1bb724b12beab03077c786a1016c6f1e 100644
--- a/src/processor_tracker_landmark_corner.h
+++ b/src/processor_tracker_landmark_corner.h
@@ -41,7 +41,7 @@ typedef std::shared_ptr<ProcessorParamsLaser> ProcessorParamsLaserPtr;
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkCorner);
-struct ProcessorParamsLaser : public ProcessorParamsBase
+struct ProcessorParamsLaser : public ProcessorParamsTrackerLandmark
{
laserscanutils::LineFinderIterativeParams line_finder_params_;
//TODO: add corner_finder_params
@@ -86,10 +86,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- ProcessorTrackerLandmarkCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
- const Scalar& _time_tolerance,
- const unsigned int& _new_corners_th,
- const unsigned int& _loop_frames_th);
+ ProcessorTrackerLandmarkCorner(ProcessorParamsLaserPtr _params_laser);
virtual ~ProcessorTrackerLandmarkCorner();
virtual void configure(SensorBasePtr _sensor) { };
diff --git a/src/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp
index e61548047ec712759974ae3e2bb9d366fb8e3a4b..c4a45acb259199c97018a20beccced79974dcea9 100644
--- a/src/processor_tracker_landmark_dummy.cpp
+++ b/src/processor_tracker_landmark_dummy.cpp
@@ -12,8 +12,8 @@
namespace wolf
{
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const Scalar _time_tolerance, const unsigned int& _max_new_features) :
- ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _time_tolerance, _max_new_features),
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
+ ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark),
n_feature_(0),
landmark_idx_non_visible_(0)
{
@@ -44,11 +44,10 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList
}
else
{
- _feature_list_out.push_back(
- std::make_shared<FeatureBase>(
- "POINT IMAGE",
- landmark_in_ptr->getDescriptor(),
- Eigen::MatrixXs::Ones(1, 1)));
+ _feature_list_out.push_back(std::make_shared<FeatureBase>(
+ "POINT IMAGE",
+ landmark_in_ptr->getDescriptor(),
+ Eigen::MatrixXs::Identity(1,1)));
_feature_landmark_correspondences[_feature_list_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
}
diff --git a/src/processor_tracker_landmark_dummy.h b/src/processor_tracker_landmark_dummy.h
index 6d6625956e71f9f3c2bbb5fd1edcea1ba5ef2ea3..6df2393fd897bad27b1fd5398bdef10d289fcc0f 100644
--- a/src/processor_tracker_landmark_dummy.h
+++ b/src/processor_tracker_landmark_dummy.h
@@ -18,7 +18,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
{
public:
- ProcessorTrackerLandmarkDummy(const Scalar _time_tolerance, const unsigned int& _max_new_features);
+ ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
virtual ~ProcessorTrackerLandmarkDummy();
virtual void configure(SensorBasePtr _sensor) { };
diff --git a/src/processor_tracker_landmark_image.cpp b/src/processor_tracker_landmark_image.cpp
index a466c6a1602c900f338b5b5adb42a5a84c8fa71b..00d90d7ff63464d075c46812275b2b3f83280245 100644
--- a/src/processor_tracker_landmark_image.cpp
+++ b/src/processor_tracker_landmark_image.cpp
@@ -1,20 +1,23 @@
#include "processor_tracker_landmark_image.h"
-#include "landmark_corner_2D.h"
-#include "landmark_AHP.h"
-#include "constraint_corner_2D.h"
+#include "capture_image.h"
#include "constraint_AHP.h"
-#include "sensor_camera.h"
-#include "pinhole_tools.h"
-#include "trajectory_base.h"
+#include "feature_base.h"
+#include "feature_point_image.h"
+#include "frame_base.h"
+#include "logging.h"
#include "map_base.h"
+#include "pinhole_tools.h"
+#include "problem.h"
+#include "sensor_camera.h"
+#include "state_block.h"
+#include "time_stamp.h"
// vision_utils
-#include <vision_utils/detectors.h>
-#include <vision_utils/descriptors.h>
-#include <vision_utils/matchers.h>
-#include <vision_utils/algorithms.h>
-
+#include <detectors.h>
+#include <descriptors.h>
+#include <matchers.h>
+#include <algorithms.h>
#include <Eigen/Geometry>
#include <iomanip> //setprecision
@@ -22,16 +25,17 @@
namespace wolf
{
-ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(const ProcessorParamsImage& _params) :
- ProcessorTrackerLandmark("IMAGE LANDMARK", _params.algorithm.time_tolerance, _params.algorithm.max_new_features),
- params_(_params),
-// active_search_grid_(),
+ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image) :
+ ProcessorTrackerLandmark("IMAGE LANDMARK", _params_tracker_landmark_image),
+ cell_width_(0),
+ cell_height_(0),
+ params_tracker_landmark_image_(_params_tracker_landmark_image),
n_feature_(0),
landmark_idx_non_visible_(0)
{
// Detector
- std::string det_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_.yaml_file_params_vision_utils);
+ std::string det_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "detector");
+ det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_landmark_image_->yaml_file_params_vision_utils);
if (det_name.compare("ORB") == 0)
det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
@@ -59,8 +63,8 @@ ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(const ProcessorPara
det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
// Descriptor
- std::string des_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_.yaml_file_params_vision_utils);
+ std::string des_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "descriptor");
+ des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_landmark_image_->yaml_file_params_vision_utils);
if (des_name.compare("ORB") == 0)
des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
@@ -86,8 +90,8 @@ ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(const ProcessorPara
des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
// Matcher
- std::string mat_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_.yaml_file_params_vision_utils);
+ std::string mat_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "matcher");
+ mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_landmark_image_->yaml_file_params_vision_utils);
if (mat_name.compare("FLANNBASED") == 0)
mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
@@ -101,7 +105,7 @@ ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(const ProcessorPara
mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
// Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_.yaml_file_params_vision_utils);
+ vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_tracker_landmark_image_->yaml_file_params_vision_utils);
active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
}
@@ -118,10 +122,10 @@ void ProcessorTrackerLandmarkImage::configure(SensorBasePtr _sensor)
active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight(), det_ptr_->getPatternRadius() );
- params_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
+ params_tracker_landmark_image_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
- cell_width_ = image_.width_ / params_activesearch_ptr_->n_cells_h;
- cell_height_ = image_.height_ / params_activesearch_ptr_->n_cells_v;
+ cell_width_ = image_.width_ / params_tracker_landmark_image_activesearch_ptr_->n_cells_h;
+ cell_height_ = image_.height_ / params_tracker_landmark_image_activesearch_ptr_->n_cells_v;
}
void ProcessorTrackerLandmarkImage::preProcess()
@@ -181,8 +185,9 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
{
FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
candidate_keypoints[cv_matches[0].trainIdx],
+ cv_matches[0].trainIdx,
candidate_descriptors.row(cv_matches[0].trainIdx),
- Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var);
+ Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
incoming_point_ptr->setTrackId(landmark_in_ptr->id());
incoming_point_ptr->setLandmarkId(landmark_in_ptr->id());
@@ -218,7 +223,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
{
return false;
-// return landmarks_tracked_ < params_.algorithm.min_features_for_keyframe;
+// return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
}
unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
@@ -245,13 +250,14 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int
index = i;
}
- if(new_keypoints[0].response > params_activesearch_ptr_->min_response_new_feature)
+ if(new_keypoints[0].response > params_tracker_landmark_image_activesearch_ptr_->min_response_new_feature)
{
list_response_.push_back(new_keypoints[0].response);
FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
new_keypoints[0],
+ 0,
new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var);
+ Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
point_ptr->setIsKnown(false);
point_ptr->setTrackId(point_ptr->id());
point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y));
@@ -284,7 +290,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
- Scalar distance = params_.algorithm.distance; // arbitrary value
+ Scalar distance = params_tracker_landmark_image_->distance; // arbitrary value
Eigen::Vector4s vec_homogeneous;
point2D = pinhole::depixellizePoint(getSensorPtr()->getIntrinsicPtr()->getState(),point2D);
@@ -364,7 +370,7 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
// world to anchor robot frame
Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getPPtr()->getState()); // sadly we cannot put a Map over a translation
- Map<const Quaternions> q_w_r0(_landmark->getAnchorFrame()->getOPtr()->getPtr());
+ const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getOPtr()->getState()));
T_W_R0 = t_w_r0 * q_w_r0;
// world to current robot frame
@@ -374,12 +380,12 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
// anchor robot to anchor camera
Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getPPtr()->getState());
- Map<const Quaternions> q_r0_c0(_landmark->getAnchorSensor()->getOPtr()->getPtr());
+ const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getOPtr()->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());
- Map<const Quaternions> q_r1_c1(this->getSensorPtr()->getOPtr()->getPtr());
+ const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensorPtr()->getOPtr()->getState()));
T_R1_C1 = t_r1_c1 * q_r1_c1;
// Transform lmk from c0 to c1 and exit
@@ -389,9 +395,8 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
Scalar ProcessorTrackerLandmarkImage::match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
{
- mat_ptr_->match(_target_descriptor, _candidate_descriptors, _cv_matches);
- Scalar normalized_score = 1 - (Scalar)(_cv_matches[0].distance)/(des_ptr_->getSize()*8);
- return normalized_score;
+ std::vector<Scalar> normalized_scores = mat_ptr_->match(_target_descriptor, _candidate_descriptors, des_ptr_->getSize(), _cv_matches);
+ return normalized_scores[0];
}
unsigned int ProcessorTrackerLandmarkImage::detect(const cv::Mat _image, cv::Rect& _roi, KeyPointVector& _new_keypoints, cv::Mat& new_descriptors)
@@ -493,7 +498,7 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
{
- ProcessorTrackerLandmarkImagePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkImage>(*(std::static_pointer_cast<ProcessorParamsImage>(_params)));
+ ProcessorTrackerLandmarkImagePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkImage>(std::static_pointer_cast<ProcessorParamsTrackerLandmarkImage>(_params));
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor_tracker_landmark_image.h b/src/processor_tracker_landmark_image.h
index 3a97599f16ca4839e9e56535d7471bb61e26d5d2..8d8b8fd425eddf59be0272304a81c623ed571fa1 100644
--- a/src/processor_tracker_landmark_image.h
+++ b/src/processor_tracker_landmark_image.h
@@ -2,25 +2,26 @@
#define PROCESSOR_TRACKER_LANDMARK_IMAGE_H
// Wolf includes
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "feature_point_image.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_tracker_landmark.h"
+
#include "landmark_AHP.h"
+#include "landmark_match.h"
#include "processor_params_image.h"
-#include "constraint_AHP.h"
+#include "processor_tracker_landmark.h"
+#include "wolf.h"
+
+#include <algorithms/activesearch/alg_activesearch.h>
+#include <descriptors/descriptor_base.h>
+#include <detectors/detector_base.h>
+#include <matchers/matcher_base.h>
-// Vision utils
-#include <vision_utils/detectors/detector_base.h>
-#include <vision_utils/descriptors/descriptor_base.h>
-#include <vision_utils/matchers/matcher_base.h>
-#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/core/mat.inl.hpp>
+#include <opencv2/core/types.hpp>
-// General includes
-#include <cmath>
-#include <complex> // std::complex, std::norm
+#include <list>
+#include <memory>
+#include <string>
+#include <vector>
namespace wolf {
@@ -38,10 +39,10 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
int cell_width_; ///< Active search cell width
int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_activesearch_ptr_; ///< Active search parameters
+ vision_utils::AlgorithmParamsACTIVESEARCHPtr params_tracker_landmark_image_activesearch_ptr_; ///< Active search parameters
protected:
- ProcessorParamsImage params_; // Struct with parameters of the processors
+ ProcessorParamsTrackerLandmarkImagePtr params_tracker_landmark_image_; // Struct with parameters of the processors
cv::Mat image_last_, image_incoming_; // Images of the "last" and "incoming" Captures
@@ -54,7 +55,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
unsigned int landmarks_tracked_ = 0;
/* pinhole params */
-// Eigen::Vector4s k_parameters_;
Eigen::Vector2s distortion_;
Eigen::Vector2s correction_;
@@ -78,7 +78,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
std::list<cv::Point> tracker_target_;
FeatureBaseList feat_lmk_found_;
- ProcessorTrackerLandmarkImage(const ProcessorParamsImage& _params);
+ ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image);
virtual ~ProcessorTrackerLandmarkImage();
virtual void configure(SensorBasePtr _sensor) ;
diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp
index 99e966840bf6b3813b35faa4a9e94a8d5810f211..00a4699ecc7c391a9b1c4b5aad2ad32f630df69e 100644
--- a/src/processor_tracker_landmark_polyline.cpp
+++ b/src/processor_tracker_landmark_polyline.cpp
@@ -167,7 +167,7 @@ unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseL
// All squared distances should be witin a threshold
// Choose the most overlapped one
- if ((dist2 < params_.position_error_th*params_.position_error_th).all() && (best_match == nullptr ||
+ if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr ||
(N_overlapped >= best_match->feature_match_to_id_-best_match->feature_match_from_id_+1 &&
dist2.mean() < best_match->normalized_score_ )))
{
@@ -244,7 +244,7 @@ unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseL
// All squared distances should be witin a threshold
// Choose the most overlapped one
- if ((dist2 < params_.position_error_th*params_.position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ ))
+ if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ ))
{
//std::cout << "BEST MATCH" << std::endl;
best_match = std::make_shared<LandmarkPolylineMatch>();
@@ -296,7 +296,7 @@ bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
//std::cout << "------------- ProcessorTrackerLandmarkPolyline::voteForKeyFrame:" << std::endl;
//std::cout << "polylines_last_.size():" << polylines_last_.size()<< std::endl;
// option 1: more than TH new features in last
- if (polylines_last_.size() >= params_.new_features_th)
+ if (polylines_last_.size() >= params_->new_features_th)
{
std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
//std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
@@ -305,7 +305,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_->getFramePtr()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getFramePtr()->id() > params_->loop_frames_th)
{
std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
return true;
@@ -609,9 +609,9 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
for (int id_lmk = lmk_last_defined_point; id_lmk > polyline_match->landmark_match_to_id_; id_lmk--)
{
//std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_.position_error_th*params_.position_error_th << std::endl;
+ //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_.position_error_th*params_.position_error_th)
+ if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
{
std::cout << "CLOSING POLYLINE" << std::endl;
@@ -704,9 +704,9 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
for (int id_lmk = lmk_first_defined_point; id_lmk < polyline_match->landmark_match_from_id_; id_lmk++)
{
//std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_.position_error_th*params_.position_error_th << std::endl;
+ //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_.position_error_th*params_.position_error_th)
+ if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
{
std::cout << "CLOSING POLYLINE" << std::endl;
@@ -892,9 +892,9 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
for (unsigned int i = 0; i < object_L.size(); i++)
{
// check configuration 1
- if(fabs(dAB-object_L[i]) < params_.position_error_th &&
- fabs(dBC-object_W[i]) < params_.position_error_th &&
- fabs(dAC-object_D[i]) < params_.position_error_th)
+ if(fabs(dAB-object_L[i]) < params_->position_error_th &&
+ fabs(dBC-object_W[i]) < params_->position_error_th &&
+ fabs(dAC-object_D[i]) < params_->position_error_th)
{
configuration = true;
classification = i;
@@ -902,9 +902,9 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
}
// check configuration 2
- if(fabs(dAB-object_W[i]) < params_.position_error_th &&
- fabs(dBC-object_L[i]) < params_.position_error_th &&
- fabs(dAC-object_D[i]) < params_.position_error_th)
+ if(fabs(dAB-object_W[i]) < params_->position_error_th &&
+ fabs(dBC-object_L[i]) < params_->position_error_th &&
+ fabs(dAC-object_D[i]) < params_->position_error_th)
{
configuration = false;
classification = i;
@@ -925,9 +925,9 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
Scalar dCD = (polyline_ptr->getPointVector(C_id) - polyline_ptr->getPointVector(D_id)).norm();
// necessary conditions
- if (fabs(dAD-dBC) > params_.position_error_th ||
- fabs(dBD-dAC) > params_.position_error_th ||
- fabs(dCD-dAB) > params_.position_error_th)
+ if (fabs(dAD-dBC) > params_->position_error_th ||
+ fabs(dBD-dAC) > params_->position_error_th ||
+ fabs(dCD-dAB) > params_->position_error_th)
continue;
}
@@ -1012,7 +1012,7 @@ ConstraintBasePtr ProcessorTrackerLandmarkPolyline::createConstraint(FeatureBase
ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
{
ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
- ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(*params);
+ ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor_tracker_landmark_polyline.h b/src/processor_tracker_landmark_polyline.h
index e4748f0f0ac9976347cf28110e843493851c2460..56d7bad9e3fe6d838901a1d70ebe0dd929a69986 100644
--- a/src/processor_tracker_landmark_polyline.h
+++ b/src/processor_tracker_landmark_polyline.h
@@ -78,7 +78,8 @@ struct LandmarkPolylineMatch : public LandmarkMatch
}
};
-struct ProcessorParamsPolyline : public ProcessorParamsTracker
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
+struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark
{
laserscanutils::LineFinderIterativeParams line_finder_params;
Scalar position_error_th;
@@ -96,7 +97,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
{
private:
laserscanutils::LineFinderIterative line_finder_;
- ProcessorParamsPolyline params_;
+ ProcessorParamsPolylinePtr params_;
FeatureBaseList polylines_incoming_;
FeatureBaseList polylines_last_;
@@ -113,7 +114,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- ProcessorTrackerLandmarkPolyline(const ProcessorParamsPolyline& _params);
+ ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params);
virtual ~ProcessorTrackerLandmarkPolyline();
virtual void configure(SensorBasePtr _sensor) { };
@@ -204,9 +205,9 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
};
-inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(const ProcessorParamsPolyline& _params) :
- ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params.time_tolerance, _params.max_new_features),
- line_finder_(_params.line_finder_params),
+inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) :
+ ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params),
+ line_finder_(_params->line_finder_params),
params_(_params),
extrinsics_transformation_computed_(false)
{
diff --git a/src/processors/CMakeLists.txt b/src/processors/CMakeLists.txt
index 3caf4f20692af864e5177e78ef4662bfa62ba133..c5902ad9c399c4055ad41fd8a5ac20fe29186933 100644
--- a/src/processors/CMakeLists.txt
+++ b/src/processors/CMakeLists.txt
@@ -4,18 +4,24 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h
- PARENT_SCOPE)
+ )
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp
- PARENT_SCOPE)
+ )
IF (vision_utils_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_feature_trifocal.h
- PARENT_SCOPE)
+ )
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_feature_trifocal.cpp
- PARENT_SCOPE)
+ )
ENDIF (vision_utils_FOUND)
+
+
+
+# These lines always at the end
+SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE)
+SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE)
diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp
index b6d57d918fc0c01cafc646146fc401d63d48d0d3..dba867cc3fd603cf913de574035d2e42a46a545d 100644
--- a/src/processors/processor_diff_drive.cpp
+++ b/src/processors/processor_diff_drive.cpp
@@ -12,14 +12,11 @@
namespace wolf
{
-ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrive ¶ms) :
- ProcessorMotion("DIFF DRIVE", 0.15, 3, 3, 3, 2, 3),
- unmeasured_perturbation_cov_(Matrix3s::Identity()*
- params.unmeasured_perturbation_std_*
- params.unmeasured_perturbation_std_),
- params_(params)
+ProcessorDiffDrive::ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params) :
+ ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 3, _params),
+ params_motion_diff_drive_(_params)
{
- //
+ unmeasured_perturbation_cov_ = Matrix3s::Identity() * params_motion_diff_drive_->unmeasured_perturbation_std * params_motion_diff_drive_->unmeasured_perturbation_std;
}
void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
@@ -84,27 +81,17 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
bool ProcessorDiffDrive::voteForKeyFrame()
{
// Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_.dist_traveled_th_)
+ if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_motion_diff_drive_->dist_traveled)
{
//WOLF_PROCESSOR_DEBUG("vote for key-frame on distance criterion.");
return true;
}
-// else
-// {
-// WOLF_PROCESSOR_DEBUG(getBuffer().get().back().delta_integr_.head<2>().norm(),
-// " < ", params_.dist_traveled_th_);
-// }
- if (std::abs(getBuffer().get().back().delta_integr_.tail<1>()(0)) > params_.theta_traveled_th_)
+ if (std::abs(getBuffer().get().back().delta_integr_.tail<1>()(0)) > params_motion_diff_drive_->angle_turned)
{
//WOLF_PROCESSOR_DEBUG("vote for key-frame on rotation criterion.");
return true;
}
-// else
-// {
-// WOLF_PROCESSOR_DEBUG(getBuffer().get().back().delta_integr_.tail<1>()(0),
-// " < ", params_.theta_traveled_th_);
-// }
return false;
}
@@ -264,7 +251,7 @@ ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
return nullptr;
}
- ProcessorBasePtr prc_ptr = std::make_shared<ProcessorDiffDrive>(*params);
+ ProcessorBasePtr prc_ptr = std::make_shared<ProcessorDiffDrive>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processors/processor_diff_drive.h b/src/processors/processor_diff_drive.h
index af9e07fb604c4551e702d01ddd07ea31adca47ab..f06b5940fe4dd3b09fcf91863347a727541d61e7 100644
--- a/src/processors/processor_diff_drive.h
+++ b/src/processors/processor_diff_drive.h
@@ -15,25 +15,21 @@ namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
-struct ProcessorParamsDiffDrive : public ProcessorParamsBase
+struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
{
- ProcessorParamsDiffDrive(const Scalar _time_tolerance,
- const Scalar _dist_travel_th,
- const Scalar _theta_traveled_th,
- const Scalar _cov_det_th,
- const Scalar _unmeasured_perturbation_std = 0.0001) :
- dist_traveled_th_(_dist_travel_th),
- theta_traveled_th_(_theta_traveled_th),
- cov_det_th_(_cov_det_th),
- unmeasured_perturbation_std_(_unmeasured_perturbation_std)
- {
- time_tolerance = _time_tolerance;
- }
-
- Scalar dist_traveled_th_;
- Scalar theta_traveled_th_;
- Scalar cov_det_th_;
- Scalar unmeasured_perturbation_std_ = 0.0001;
+// ProcessorParamsDiffDrive(const Scalar _time_tolerance,
+// const Scalar _dist_travel_th,
+// const Scalar _theta_traveled_th,
+// const Scalar _cov_det_th,
+// const Scalar _unmeasured_perturbation_std = 0.0001) :
+// dist_traveled_th_(_dist_travel_th),
+// theta_traveled_th_(_theta_traveled_th),
+// cov_det_th_(_cov_det_th),
+// unmeasured_perturbation_std_(_unmeasured_perturbation_std)
+// {
+// time_tolerance = _time_tolerance;
+// }
+ Scalar unmeasured_perturbation_std = 0.0001;
};
/**
@@ -55,7 +51,7 @@ class ProcessorDiffDrive : public ProcessorMotion
{
public:
- ProcessorDiffDrive(const ProcessorParamsDiffDrive& params);
+ ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params);
virtual ~ProcessorDiffDrive() = default;
virtual void configure(SensorBasePtr _sensor) override { }
@@ -64,11 +60,9 @@ public:
protected:
- /// @brief Covariance to be added to the unmeasured perturbation.
- Matrix3s unmeasured_perturbation_cov_;
-
/// @brief Intrinsic params
- ProcessorParamsDiffDrive params_;
+ ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
+ MatrixXs unmeasured_perturbation_cov_;
virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
diff --git a/src/processors/processor_tracker_feature_trifocal.cpp b/src/processors/processor_tracker_feature_trifocal.cpp
index e6de65c4f57b60e64e9ecbf5374bc8614269ede9..b8ef70d065b3e766bfb2b2877c5d7e26f1c3cdee 100644
--- a/src/processors/processor_tracker_feature_trifocal.cpp
+++ b/src/processors/processor_tracker_feature_trifocal.cpp
@@ -5,31 +5,47 @@
#include "sensor_camera.h"
#include "feature_point_image.h"
#include "constraints/constraint_autodiff_trifocal.h"
+#include "capture_image.h"
// vision_utils
-#include <vision_utils/detectors.h>
-#include <vision_utils/descriptors.h>
-#include <vision_utils/matchers.h>
-#include <vision_utils/algorithms.h>
+#include <vision_utils.h>
+#include <detectors.h>
+#include <descriptors.h>
+#include <matchers.h>
+#include <algorithms.h>
+#include <memory>
namespace wolf {
+
+//// DEBUG =====================================
+//debug_tTmp = clock();
+//WOLF_TRACE("======== DetectNewFeatures =========");
+//// ===========================================
+//
+//// DEBUG =====================================
+//debug_comp_time_ = (double)(clock() - debug_tTmp) / CLOCKS_PER_SEC;
+//WOLF_TRACE("--> TIME: Detect New Features: detect ",debug_comp_time_);
+//// ===========================================
+
+
// Constructor
-ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(const ProcessorParamsTrackerFeatureTrifocal& _params) :
- ProcessorTrackerFeature("TRACKER FEATURE TRIFOCAL", _params.time_tolerance, _params.max_new_features ),
- cell_width_(0), cell_height_(0), // These will be initialized to proper values taken from the sensor via function configure()
- params_(_params),
+ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal) :
+ ProcessorTrackerFeature("TRACKER FEATURE TRIFOCAL", _params_tracker_feature_trifocal ),
+ params_tracker_feature_trifocal_(_params_tracker_feature_trifocal),
+ capture_last_(nullptr),
+ capture_incoming_(nullptr),
prev_origin_ptr_(nullptr),
initialized_(false)
{
- setName(_params.name);
- assert(!(params_.yaml_file_params_vision_utils.empty()) && "Missing YAML file with vision_utils parameters!");
- assert(file_exists(params_.yaml_file_params_vision_utils) && "Cannot setup processor: vision_utils' YAML file does not exist.");
+ setName(_params_tracker_feature_trifocal->name);
+ assert(!(params_tracker_feature_trifocal_->yaml_file_params_vision_utils.empty()) && "Missing YAML file with vision_utils parameters!");
+ assert(file_exists(params_tracker_feature_trifocal_->yaml_file_params_vision_utils) && "Cannot setup processor: vision_utils' YAML file does not exist.");
// Detector
- std::string det_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_.yaml_file_params_vision_utils);
+ std::string det_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "detector");
+ det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_feature_trifocal_->yaml_file_params_vision_utils);
if (det_name.compare("ORB") == 0)
det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
@@ -57,8 +73,8 @@ ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(const Processor
det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
// Descriptor
- std::string des_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_.yaml_file_params_vision_utils);
+ std::string des_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "descriptor");
+ des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_feature_trifocal_->yaml_file_params_vision_utils);
if (des_name.compare("ORB") == 0)
des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
@@ -84,8 +100,8 @@ ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(const Processor
des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
// Matcher
- std::string mat_name = vision_utils::readYamlType(params_.yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_.yaml_file_params_vision_utils);
+ std::string mat_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "matcher");
+ mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_trifocal_->yaml_file_params_vision_utils);
if (mat_name.compare("FLANNBASED") == 0)
mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
@@ -98,128 +114,183 @@ ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(const Processor
if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0)
mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
- // Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_.yaml_file_params_vision_utils);
- active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
+ // DEBUG VIEW
+ cv::startWindowThread();
+ cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL);
}
// Destructor
ProcessorTrackerFeatureTrifocal::~ProcessorTrackerFeatureTrifocal()
{
+// cv::destroyAllWindows();
}
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out)
+bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, const cv::KeyPoint& _kp_incoming)
{
- unsigned int n_new_features = 0;
+ // List of conditions
+ bool inlier = true;
+
+ // A. Check euclidean norm
+ inlier = inlier && (cv::norm(_kp_incoming.pt-_kp_last.pt) < params_tracker_feature_trifocal_->max_euclidean_distance);
+
+ return inlier;
+}
- KeyPointVector kps;
- cv::Mat desc;
- cv::Rect roi;
- KeyPointVector new_keypoints;
- cv::Mat new_descriptors;
- cv::KeyPointsFilter keypoint_filter;
- for (unsigned int n_iterations = 0; _max_new_features == 0 || n_iterations < _max_new_features; n_iterations++)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out)
+{
+// // DEBUG =====================================
+// clock_t debug_tStart;
+// double debug_comp_time_;
+// debug_tStart = clock();
+// WOLF_TRACE("======== DetectNewFeatures =========");
+// // ===========================================
+
+ for (unsigned int n_iterations = 0; n_iterations < _max_new_features; ++n_iterations)
{
- if (active_search_ptr_->pickEmptyRoi(roi))
+ Eigen::Vector2i cell_last;
+ if (capture_last_->grid_features_->pickEmptyTrackingCell(cell_last))
{
- kps = det_ptr_->detect (image_last_, roi);
- desc = des_ptr_->getDescriptor(image_last_, kps);
+ // Get best keypoint in cell
+ vision_utils::FeatureIdxMap cell_feat_map = capture_last_->grid_features_->getFeatureIdxMap(cell_last(0), cell_last(1));
+ bool found_feature_in_cell = false;
- if (kps.size() > 0)
+ for (auto target_last_pair_response_idx : cell_feat_map)
{
- KeyPointVector list_keypoints = kps;
- unsigned int index = 0;
- keypoint_filter.retainBest(kps,1);
- for(unsigned int i = 0; i < list_keypoints.size(); i++)
- {
- if(list_keypoints[i].pt == kps[0].pt)
- index = i;
- }
- if(kps[0].response > params_activesearch_ptr_->min_response_new_feature)
+ // Get last KeyPoint
+ unsigned int index_last = target_last_pair_response_idx.second;
+ cv::KeyPoint kp_last = capture_last_->keypoints_.at(index_last);
+ assert(target_last_pair_response_idx.first == kp_last.response && "[ProcessorTrackerFeatureTrifocal::detectNewFeatures]: response mismatch.");
+
+ // Check if there is match with incoming, if not we do not want it
+ if (capture_last_->map_index_to_next_.count(index_last))
{
- std::cout << "response: " << kps[0].response << std::endl;
- FeaturePointImagePtr point = std::make_shared<FeaturePointImage>(
- kps[0],
- desc.row(index),
- Eigen::Matrix2s::Identity() * params_.pixel_noise_std * params_.pixel_noise_std);
- point->setIsKnown(false);
- _feature_list_out.push_back(point);
+ unsigned int index_incoming = capture_last_->map_index_to_next_[index_last];
+ cv::KeyPoint kp_incoming = capture_incoming_->keypoints_.at(index_incoming);
+
+ if (isInlier( kp_incoming, kp_last))
+ {
+ // Create WOLF feature
+ FeaturePointImagePtr last_point_ptr = std::make_shared<FeaturePointImage>(
+ kp_last,
+ index_last,
+ capture_last_->descriptors_.row(index_last),
+ Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
+
+ _feature_list_out.push_back(last_point_ptr);
- active_search_ptr_->hitCell(kps[0]);
+ // hit cell to acknowledge there's a tracked point in that cell
+ capture_last_->grid_features_->hitTrackingCell(kp_last);
- n_new_features++;
+ found_feature_in_cell = true;
+
+ break; // Good kp found for this grid.
+ }
}
}
- else
- active_search_ptr_->blockCell(roi);
+ if (!found_feature_in_cell)
+ capture_last_->grid_features_->blockTrackingCell(cell_last);
}
else
- break;
+ break; // There are no empty cells
}
- WOLF_DEBUG( "DetectNewFeatures - Number of new features detected: " , n_new_features );
- return n_new_features;
-}
+ WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _feature_list_out.size() );
-bool ProcessorTrackerFeatureTrifocal::voteForKeyFrame()
-{
- bool vote = true;
-
- // list of conditions with AND logic (all need to be satisfied)
-
- // 1. vote if we had enough features before
- vote = vote && getNewFeaturesListLast().size() >= params_.min_features_for_keyframe;
- // 2. vote if we have not enough features now
- vote = vote && getNewFeaturesListIncoming().size() < params_.min_features_for_keyframe;
+// // DEBUG
+// debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC;
+// WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_);
+// WOLF_TRACE("======== ========= =========");
- return vote;
-}
-
-bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
-{
- return true;
+ return _feature_list_out.size();
}
unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches)
{
- KeyPointVector kps;
- cv::Mat desc;
- KeyPointVector candidate_keypoints;
- cv::Mat candidate_descriptors;
- cv::DMatch cv_match;
-
- for (auto feature_base_ptr : _feature_list_in)
+// // DEBUG =====================================
+// clock_t debug_tStart;
+// double debug_comp_time_;
+// debug_tStart = clock();
+// WOLF_TRACE("======== TrackFeatures =========");
+// // ===========================================
+
+ for (auto feature_base_last_ : _feature_list_in)
{
- FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr);
+ FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_);
- cv::Rect roi = vision_utils::setRoi(feature_ptr->getKeypoint().pt.x, feature_ptr->getKeypoint().pt.y, cell_width_, cell_height_);
+ if ( capture_last_->map_index_to_next_.count(feature_last_->getIndexKeyPoint()) )
+ {
+ int index_kp_incoming = capture_last_->map_index_to_next_[feature_last_->getIndexKeyPoint()];
- active_search_ptr_->hitCell(feature_ptr->getKeypoint());
+ if (capture_incoming_->matches_normalized_scores_.at(index_kp_incoming) > mat_ptr_->getParams()->min_norm_score )
+ {
+ // Check Euclidean distance between keypoints
+ cv::KeyPoint kp_incoming = capture_incoming_->keypoints_.at(index_kp_incoming);
+ cv::KeyPoint kp_last = capture_last_->keypoints_.at(feature_last_->getIndexKeyPoint());
- cv::Mat target_descriptor = feature_ptr->getDescriptor();
+ if (isInlier(kp_last, kp_incoming))
+ {
+ FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
+ kp_incoming,
+ index_kp_incoming,
+ capture_incoming_->descriptors_.row(index_kp_incoming),
+ Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
- kps = det_ptr_->detect(image_incoming_, roi);
+ _feature_list_out.push_back(incoming_point_ptr);
- if (kps.size() > 0)
- {
- desc = des_ptr_->getDescriptor(image_incoming_, kps);
- Scalar normalized_score = mat_ptr_->match(target_descriptor, desc, des_ptr_->getSize(), cv_match);
- if ( normalized_score > mat_ptr_->getParams()->min_norm_score )
- {
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_match.trainIdx], (candidate_descriptors.row(cv_match.trainIdx)),
- Eigen::Matrix2s::Identity() * params_.pixel_noise_std * params_.pixel_noise_std);
- incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
- _feature_list_out.push_back(incoming_point_ptr);
+ _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_last_, capture_incoming_->matches_normalized_scores_.at(index_kp_incoming)}));
- _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
+ // hit cell to acknowledge there's a tracked point in that cell
+ capture_incoming_->grid_features_->hitTrackingCell(kp_incoming);
+ }
}
}
}
+
+// // DEBUG
+// debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC;
+// WOLF_TRACE("--> TIME: track: ",debug_comp_time_);
+// WOLF_TRACE("======== ========= =========");
+
+ WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _feature_list_out.size() );
+
return _feature_list_out.size();
}
+bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
+{
+ return true;
+}
+
+bool ProcessorTrackerFeatureTrifocal::voteForKeyFrame()
+{
+ // A. crossing voting threshold with ascending number of features
+ bool vote_up = true;
+ // 1. vote if we did not have enough features before
+ vote_up = vote_up && (previousNumberOfTracks() < params_tracker_feature_trifocal_->min_features_for_keyframe);
+ // 2. vote if we have enough features now
+ vote_up = vote_up && (incoming_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe);
+
+ // B. crossing voting threshold with descending number of features
+ bool vote_down = true;
+ // 1. vote if we had enough features before
+ vote_down = vote_down && (last_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe);
+ // 2. vote if we have not enough features now
+ vote_down = vote_down && (incoming_ptr_->getFeatureList().size() < params_tracker_feature_trifocal_->min_features_for_keyframe);
+
+ if (vote_up)
+ WOLF_TRACE("VOTE UP");
+ if (vote_down)
+ WOLF_TRACE("VOTE DOWN");
+
+ return vote_up || vote_down;
+}
+
+void ProcessorTrackerFeatureTrifocal::advanceDerived()
+{
+ ProcessorTrackerFeature::advanceDerived();
+}
+
void ProcessorTrackerFeatureTrifocal::resetDerived()
{
// Call parent class method
@@ -229,7 +300,6 @@ void ProcessorTrackerFeatureTrifocal::resetDerived()
if (initialized_)
prev_origin_ptr_ = origin_ptr_;
-
// // Print resulting list
// TrackMatches matches_prevorig_origin = track_matrix_.matches(prev_origin_ptr_, origin_ptr_);
// for (auto const & pair_trkid_pair : matches_prevorig_origin)
@@ -241,11 +311,12 @@ void ProcessorTrackerFeatureTrifocal::resetDerived()
// " prev: ", feature_in_prev->id(),
// " origin: ", feature_in_origin->id());
// }
-
}
void ProcessorTrackerFeatureTrifocal::preProcess()
{
+ WOLF_TRACE("-------- Image ", getIncomingPtr()->id(), " -- t = ", getIncomingPtr()->getTimeStamp(), " s ----------");
+
if (!initialized_)
{
if (origin_ptr_ && last_ptr_ && (last_ptr_ != origin_ptr_) && prev_origin_ptr_ == nullptr)
@@ -254,6 +325,66 @@ void ProcessorTrackerFeatureTrifocal::preProcess()
if (prev_origin_ptr_ && origin_ptr_ && last_ptr_ && prev_origin_ptr_ != origin_ptr_)
initialized_ = true;
}
+
+ // Get capture
+ capture_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
+
+ // Detect INC KeyPoints
+ capture_incoming_->keypoints_ = det_ptr_->detect(capture_incoming_->getImage());
+
+ // Get INC descriptors
+ capture_incoming_->descriptors_ = des_ptr_->getDescriptor(capture_incoming_->getImage(), capture_incoming_->keypoints_);
+
+ // Create and fill incoming grid
+ capture_incoming_->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(capture_incoming_->getImage().rows, capture_incoming_->getImage().cols, params_tracker_feature_trifocal_->n_cells_v, params_tracker_feature_trifocal_->n_cells_h);
+
+ capture_incoming_->grid_features_->insert(capture_incoming_->keypoints_);
+
+ // If last_ptr_ is not null, then we can do some computation here.
+ if (last_ptr_ != nullptr)
+ {
+ // Get capture
+ capture_last_ = std::static_pointer_cast<CaptureImage>(last_ptr_);
+
+ // Match full image (faster)
+ // We exchange the order of the descriptors to fill better the map hereafter (map does not allow a single key)
+ DMatchVector matches_incoming_from_last;
+
+ capture_incoming_->matches_normalized_scores_ = mat_ptr_->robustMatch(capture_incoming_->keypoints_,
+ capture_last_->keypoints_,
+ capture_incoming_->descriptors_,
+ capture_last_->descriptors_,
+ des_ptr_->getSize(),
+ matches_incoming_from_last);
+
+ capture_incoming_->matches_from_precedent_ = matches_incoming_from_last;
+
+ // Set capture map of match indices
+ for (auto match : capture_incoming_->matches_from_precedent_)
+ capture_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
+ }
+}
+
+void ProcessorTrackerFeatureTrifocal::postProcess()
+{
+ // DEBUG
+ std::vector<vision_utils::KeyPointEnhanced> kps_e;
+
+ for (auto const & feat_base : last_ptr_->getFeatureList())
+ {
+ FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ unsigned int id = feat->id();
+ unsigned int track_id = feat->trackId();
+ vision_utils::KeyPointEnhanced kp_e(feat->getKeypoint(), id, track_id, track_matrix_.trackSize(track_id), feat->getMeasurementCovariance());
+ kps_e.push_back(kp_e);
+ }
+
+ // DEBUG
+ cv::Mat img = (std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage();
+ cv::Mat img_proc = vision_utils::buildImageProcessed(img, kps_e);
+
+ cv::imshow("DEBUG VIEW", img_proc);
+ cv::waitKey(1);
}
ConstraintBasePtr ProcessorTrackerFeatureTrifocal::createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
@@ -274,41 +405,41 @@ void ProcessorTrackerFeatureTrifocal::establishConstraints()
TrackMatches matches = track_matrix_.matches(prev_origin_ptr_, last_ptr_); // it's guaranteed by construction that the track also includes origin
for (auto pair_trkid_match : matches) // OMG this will add potentially a loooot of constraints! TODO see a smarter way of adding constraints
- {
+ { // Currently reduced by creating constraints for large tracks
// get track ID
size_t trk_id = pair_trkid_match.first;
- // get the three features for this track
- // FeatureBasePtr ftr_prev = track_matrix_.feature(trk_id, prev_origin_ptr_); // left here for ref, but implemented in a quicker way below
- // FeatureBasePtr ftr_last = track_matrix_.feature(trk_id, last_ptr_); // same here
- FeatureBasePtr ftr_prev = pair_trkid_match.second.first;
- FeatureBasePtr ftr_orig = track_matrix_.feature(trk_id, origin_ptr_); // because it's a tracker, this feature in the middle of prev and last exists for sure!
- FeatureBasePtr ftr_last = pair_trkid_match.second.second;
-
- // make constraint
- ConstraintAutodiffTrifocalPtr ctr = std::make_shared<ConstraintAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, CTR_ACTIVE);
-
- // link to wolf tree
- ftr_last->addConstraint(ctr);
- ftr_orig->addConstrainedBy(ctr);
- ftr_prev->addConstrainedBy(ctr);
+ if (track_matrix_.trackSize(trk_id)>params_tracker_feature_trifocal_->min_track_length_for_constraint)
+ {
+ // get the three features for this track
+ // FeatureBasePtr ftr_prev = track_matrix_.feature(trk_id, prev_origin_ptr_); // left here for ref, but implemented in a quicker way below
+ // FeatureBasePtr ftr_last = track_matrix_.feature(trk_id, last_ptr_); // same here
+ FeatureBasePtr ftr_prev = pair_trkid_match.second.first;
+ FeatureBasePtr ftr_orig = track_matrix_.feature(trk_id, origin_ptr_); // because it's a tracker, this feature in the middle of prev and last exists for sure!
+ FeatureBasePtr ftr_last = pair_trkid_match.second.second;
+
+ // make constraint
+ ConstraintAutodiffTrifocalPtr ctr = std::make_shared<ConstraintAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, CTR_ACTIVE);
+
+ // link to wolf tree
+ ftr_last->addConstraint(ctr);
+ ftr_orig->addConstrainedBy(ctr);
+ ftr_prev->addConstrainedBy(ctr);
+ }
}
}
}
+void ProcessorTrackerFeatureTrifocal::setParams(const ProcessorParamsTrackerFeatureTrifocalPtr _params)
+{
+ params_tracker_feature_trifocal_ = _params;
+}
+
void ProcessorTrackerFeatureTrifocal::configure(SensorBasePtr _sensor)
{
SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(_sensor);
- camera->setNoiseStd(Vector2s::Ones() * params_.pixel_noise_std);
-
- active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight() , det_ptr_->getPatternRadius());
-
- params_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
-
- cell_width_ = camera->getImgWidth() / params_activesearch_ptr_->n_cells_h;
- cell_height_ = camera->getImgHeight() / params_activesearch_ptr_->n_cells_v;
-
+ camera->setNoiseStd(Vector2s::Ones() * params_tracker_feature_trifocal_->pixel_noise_std);
}
ProcessorBasePtr ProcessorTrackerFeatureTrifocal::create(const std::string& _unique_name,
@@ -317,7 +448,7 @@ ProcessorBasePtr ProcessorTrackerFeatureTrifocal::create(const std::string& _uni
{
const auto params = std::static_pointer_cast<ProcessorParamsTrackerFeatureTrifocal>(_params);
- ProcessorBasePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureTrifocal>(*params);
+ ProcessorBasePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureTrifocal>(params);
prc_ptr->setName(_unique_name);
prc_ptr->configure(_sensor_ptr);
return prc_ptr;
diff --git a/src/processors/processor_tracker_feature_trifocal.h b/src/processors/processor_tracker_feature_trifocal.h
index e7a648308ddec03bb6a20b00c7f94386d779875b..e31ee6f5f439d8cc51eec4ebad2250ff94c6d8fb 100644
--- a/src/processors/processor_tracker_feature_trifocal.h
+++ b/src/processors/processor_tracker_feature_trifocal.h
@@ -3,53 +3,48 @@
//Wolf includes
#include "../processor_tracker_feature.h"
+#include "../capture_image.h"
// Vision utils
-#include <vision_utils/detectors/detector_base.h>
-#include <vision_utils/descriptors/descriptor_base.h>
-#include <vision_utils/matchers/matcher_base.h>
-#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils.h>
+#include <detectors/detector_base.h>
+#include <descriptors/descriptor_base.h>
+#include <matchers/matcher_base.h>
+#include <algorithms/activesearch/alg_activesearch.h>
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureTrifocal);
-struct ProcessorParamsTrackerFeatureTrifocal : public ProcessorParamsTracker
+struct ProcessorParamsTrackerFeatureTrifocal : public ProcessorParamsTrackerFeature
{
std::string yaml_file_params_vision_utils;
- unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
-
+ int n_cells_h;
+ int n_cells_v;
+ int min_response_new_feature;
+ Scalar max_euclidean_distance;
Scalar pixel_noise_std; ///< std noise of the pixel
+ int min_track_length_for_constraint; ///< Minimum track length of a matched feature to create a constraint
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureTrifocal);
class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
{
-
// Parameters for vision_utils
protected:
vision_utils::DetectorBasePtr det_ptr_;
vision_utils::DescriptorBasePtr des_ptr_;
vision_utils::MatcherBasePtr mat_ptr_;
- vision_utils::AlgorithmACTIVESEARCHPtr active_search_ptr_; // Active Search
-
- int cell_width_; ///< Active search cell width
- int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_activesearch_ptr_; ///< Active search parameters
protected:
- ProcessorParamsTrackerFeatureTrifocal params_; ///< Configuration parameters
- cv::Mat image_last_, image_incoming_; ///< Images of the "last" and "incoming" Captures
+ ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal_; ///< Configuration parameters
- struct
- {
- unsigned int width_; ///< width of the image
- unsigned int height_; ///< height of the image
- } image_;
+ CaptureImagePtr capture_last_;
+ CaptureImagePtr capture_incoming_;
private:
CaptureBasePtr prev_origin_ptr_; ///< Capture previous to origin_ptr_ for the third focus of the trifocal.
@@ -59,7 +54,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
/** \brief Class constructor
*/
- ProcessorTrackerFeatureTrifocal( const ProcessorParamsTrackerFeatureTrifocal& _params );
+ ProcessorTrackerFeatureTrifocal( ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal );
/** \brief Class Destructor
*/
@@ -112,6 +107,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief advance pointers
+ *
+ */
+ virtual void advanceDerived() override;
+
/** \brief reset pointers and match lists at KF creation
*
*/
@@ -122,12 +122,21 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*/
virtual void preProcess() override;
+ /** \brief Post-process
+ *
+ */
+ virtual void postProcess() override;
+
/** \brief Establish constraints between features in Captures \b last and \b origin
*/
virtual void establishConstraints() override;
CaptureBasePtr getPrevOriginPtr();
+ bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
+
+ void setParams(const ProcessorParamsTrackerFeatureTrifocalPtr _params);
+
public:
/// @brief Factory method
diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp
index 4398d8e3f2250ce31184579ec4c530425c1c3ac9..8c448046228b32bf79494fd77d3995e8bab854f8 100644
--- a/src/sensor_GPS_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -28,11 +28,6 @@ SensorGPSFix::~SensorGPSFix()
//
}
-Scalar SensorGPSFix::getNoise() const
-{
- return noise_std_(0);
-}
-
// Define the factory method
SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
const IntrinsicsBasePtr _intrinsics)
diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h
index 3291aa538a7f7cd7a8fa838df8190a4b70ab4acb..c319d159f0adce4734f832a24c40ba361c8ba64b 100644
--- a/src/sensor_GPS_fix.h
+++ b/src/sensor_GPS_fix.h
@@ -38,14 +38,7 @@ class SensorGPSFix : public SensorBase
SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr);
virtual ~SensorGPSFix();
-
- /** \brief Returns noise standard deviation
- *
- * Returns noise standard deviation
- *
- **/
- Scalar getNoise() const;
-
+
public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
diff --git a/src/sensor_base.h b/src/sensor_base.h
index cfacfbc7d23bd17eb34b0ef3bff6fa3cc95ae207..beb4924e9dbb0de83a8426b2b89af3a26bd65478 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -149,6 +149,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void setNoiseCov(const Eigen::MatrixXs & _noise_std);
Eigen::VectorXs getNoiseStd();
Eigen::MatrixXs getNoiseCov();
+ void setExtrinsicDynamic(bool _extrinsic_dynamic);
+ void setIntrinsicDynamic(bool _intrinsic_dynamic);
protected:
Size computeCalibSize() const;
@@ -260,6 +262,16 @@ inline void SensorBase::updateCalibSize()
calib_size_ = computeCalibSize();
}
+inline void SensorBase::setExtrinsicDynamic(bool _extrinsic_dynamic)
+{
+ extrinsic_dynamic_ = _extrinsic_dynamic;
+}
+
+inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
+{
+ intrinsic_dynamic_ = _intrinsic_dynamic;
+}
+
} // namespace wolf
diff --git a/src/sensors/CMakeLists.txt b/src/sensors/CMakeLists.txt
index 5b634817ee7ef1b81e67cdf29af8ed8a227b131a..48a27e6003eeba7d9bff033c682a187057d61789 100644
--- a/src/sensors/CMakeLists.txt
+++ b/src/sensors/CMakeLists.txt
@@ -6,8 +6,15 @@ SET(HDRS_SENSOR ${HDRS_SENSOR}
${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h
- PARENT_SCOPE)
+ )
SET(SRCS_SENSOR ${SRCS_SENSOR}
${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp
- PARENT_SCOPE)
+ )
+
+
+
+# These lines always at the end
+SET(HDRS_SENSOR ${HDRS_SENSOR} PARENT_SCOPE)
+SET(SRCS_SENSOR ${SRCS_SENSOR} PARENT_SCOPE)
+
\ No newline at end of file
diff --git a/src/serialization/cereal/serialization_processor_odom2d_params.h b/src/serialization/cereal/serialization_processor_odom2d_params.h
index 109fa6c54f8ab5a4f583fb3c3e360124a967dc40..4e34becad92d9918c9d8cfb7b8d571a343ef4b82 100644
--- a/src/serialization/cereal/serialization_processor_odom2d_params.h
+++ b/src/serialization/cereal/serialization_processor_odom2d_params.h
@@ -14,12 +14,12 @@ void serialize(Archive& ar, wolf::ProcessorParamsOdom2D& o,
ar( cereal::make_nvp("ProcessorParamsBase",
cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
- ar( cereal::make_nvp("cov_det_th_", o.cov_det_th_) );
+ ar( cereal::make_nvp("cov_det_th_", o.cov_det) );
ar( cereal::make_nvp("dist_traveled_th_", o.dist_traveled_th_) );
ar( cereal::make_nvp("elapsed_time_th_", o.elapsed_time_th_) );
ar( cereal::make_nvp("theta_traveled_th_", o.theta_traveled_th_) );
ar( cereal::make_nvp("unmeasured_perturbation_std_",
- o.unmeasured_perturbation_std_) );
+ o.unmeasured_perturbation_std) );
}
} // namespace cereal
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 078d304265420ff86f7c43d5c079fcd2aaae2779..d69f8fd098651d6162e61d470cacb61390561199 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -48,6 +48,10 @@ target_link_libraries(gtest_capture_base ${PROJECT_NAME})
#wolf_add_gtest(gtest_constraint_sparse gtest_constraint_sparse.cpp)
#target_link_libraries(gtest_constraint_sparse ${PROJECT_NAME})
+# ConstraintAutodiff class test
+wolf_add_gtest(gtest_constraint_autodiff gtest_constraint_autodiff.cpp)
+target_link_libraries(gtest_constraint_autodiff ${PROJECT_NAME})
+
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
@@ -184,10 +188,6 @@ target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
-# ConstraintAutodiff class test
-wolf_add_gtest(gtest_constraint_autodiff gtest_constraint_autodiff.cpp)
-target_link_libraries(gtest_constraint_autodiff ${PROJECT_NAME})
-
# ------- Now Core classes Serialization ----------
add_subdirectory(serialization)
diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp
index 38dfbc223a2e3137c503eef65ff343e379265365..5aa518eb9475a0fd1a05daf938c5443cc9658a3b 100644
--- a/src/test/gtest_IMU.cpp
+++ b/src/test/gtest_IMU.cpp
@@ -631,14 +631,16 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml" );
ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/src/examples/processor_odom_3D.yaml");
sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
+
processor_odo = static_pointer_cast<ProcessorOdom3D>(processor);
+
processor_odo->setTimeTolerance(dt/2);
// prevent this processor from voting by setting high thresholds :
- processor_odo->setAngleTurned(2.0);
- processor_odo->setDistTraveled(1.0);
- processor_odo->setMaxBuffLength(10);
- processor_odo->setMaxTimeSpan(1.0);
+ processor_odo->setAngleTurned(999);
+ processor_odo->setDistTraveled(999);
+ processor_odo->setMaxBuffLength(999);
+ processor_odo->setMaxTimeSpan(999);
}
virtual void integrateAll() override
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index 80a6579ae459d7932e7c0e4df9b95e48a76ce326..a0587e29ce96207c1e7bdaa05fb4f7e4a9b5cbc4 100644
--- a/src/test/gtest_constraint_absolute.cpp
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -41,7 +41,7 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
// Capture, feature and constraint
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose10, 10, 9, nullptr));
+CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
////////////////////////////////////////////////////////
/* In the tests below, we check that ConstraintBlockAbsolute and ConstraintQuaternionAbsolute are working fine
@@ -51,7 +51,7 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullpt
TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
);
@@ -60,7 +60,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
);
@@ -78,7 +78,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
);
@@ -87,7 +87,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
);
@@ -105,7 +105,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
);
@@ -114,7 +114,7 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
);
diff --git a/src/test/gtest_constraint_autodiff_trifocal.cpp b/src/test/gtest_constraint_autodiff_trifocal.cpp
index f4c8bc89905b7263105ab75e7197f3989dd00096..aa34b7ea76b6efb83d3cbf69b75057af5fc66e38 100644
--- a/src/test/gtest_constraint_autodiff_trifocal.cpp
+++ b/src/test/gtest_constraint_autodiff_trifocal.cpp
@@ -114,27 +114,20 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
// Build problem
problem = Problem::create("PO 3D");
ceres::Solver::Options options;
-// options.function_tolerance = 1e-16;
-// options.max_num_iterations = 500;
-// options.parameter_tolerance = 1e-16;
-// options.max_linear_solver_iterations = 500;
-// options.min_linear_solver_iterations = 20;
-// options.use_inner_iterations = true;
-// options.gradient_tolerance = 1e-16;
ceres_manager = std::make_shared<CeresManager>(problem, options);
// Install sensor and processor
S = problem->installSensor("CAMERA", "canonical", pose_cam, wolf_root + "/src/examples/camera_params_canonical.yaml");
camera = std::static_pointer_cast<SensorCamera>(S);
- ProcessorParamsTrackerFeatureTrifocalPtr params_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
- params_trifocal->time_tolerance = 1.0/2;
- params_trifocal->max_new_features = 5;
- params_trifocal->min_features_for_keyframe = 5;
- params_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/vision_utils_active_search.yaml";
+ ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
+ params_tracker_feature_trifocal_trifocal->time_tolerance = 1.0/2;
+ params_tracker_feature_trifocal_trifocal->max_new_features = 5;
+ params_tracker_feature_trifocal_trifocal->min_features_for_keyframe = 5;
+ params_tracker_feature_trifocal_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/vision_utils_active_search.yaml";
- proc_trifocal = std::make_shared<ProcessorTrackerFeatureTrifocal>(*params_trifocal);
- camera->addProcessor(proc_trifocal);
+ ProcessorBasePtr proc = problem->installProcessor("TRACKER FEATURE TRIFOCAL", "trifocal", camera, params_tracker_feature_trifocal_trifocal);
+ proc_trifocal = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(proc);
// Add three viewpoints with frame, capture and feature
Vector2s pix(0,0);
@@ -346,10 +339,19 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
Vector3s res;
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("Initial state: ", F1->getState().transpose());
@@ -362,10 +364,13 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
pose_perturbated.segment(3,4).normalize();
F1->setState(pose_perturbated);
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ F1_p = F1->getPPtr()->getState();
+ F1_o = F1->getOPtr()->getState();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
@@ -378,12 +383,21 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
F2->fix();
F3->fix();
- std::string report = ceres_manager->solve(1);
-
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("solved state: ", F1->getState().transpose());
@@ -407,10 +421,19 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
Vector3s res;
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("Initial state: ", F2->getState().transpose());
@@ -423,10 +446,13 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
pose_perturbated.segment(3,4).normalize();
F2->setState(pose_perturbated);
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ F2_p = F2->getPPtr()->getState();
+ F2_o = F2->getOPtr()->getState();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
@@ -439,12 +465,21 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
F2->unfix();
F3->fix();
- std::string report = ceres_manager->solve(1);
-
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("solved state: ", F2->getState().transpose());
@@ -468,10 +503,19 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
Vector3s res;
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("Initial state: ", F3->getState().transpose());
@@ -484,10 +528,13 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
pose_perturbated.segment(3,4).normalize();
F3->setState(pose_perturbated);
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ F3_p = F3->getPPtr()->getState();
+ F3_o = F3->getOPtr()->getState();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
@@ -501,12 +548,21 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
F2->fix();
F3->unfix();
- std::string report = ceres_manager->solve(1);
-
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("solved state: ", F3->getState().transpose());
@@ -530,10 +586,19 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
Vector3s res;
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("Initial state: ", S->getPPtr()->getState().transpose(), " ", S->getOPtr()->getState().transpose());
@@ -549,10 +614,13 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
S->getPPtr()->setState(pos_perturbated);
S->getOPtr()->setState(ori_perturbated);
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ S_p = S->getPPtr()->getState();
+ S_o = S->getOPtr()->getState();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
@@ -565,12 +633,21 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
F2->fix();
F3->fix();
- std::string report = ceres_manager->solve(1);
-
- c123->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ 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();
+
+ c123->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
WOLF_DEBUG("solved state: ", S->getPPtr()->getState().transpose(), " ", S->getOPtr()->getState().transpose());
@@ -676,7 +753,7 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
F3->getPPtr()->setState( pos3 + 0.2*Vector3s::Random());
F3->getOPtr()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
- std::string report = ceres_manager->solve(1);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
// Print results
WOLF_DEBUG("report: ", report);
@@ -688,14 +765,23 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
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();
+
// evaluate residuals
Vector3s res;
for (size_t i=0; i<cv123.size(); i++)
{
- cv123.at(i)->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ cv123.at(i)->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-10);
@@ -733,7 +819,7 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
F3->getPPtr()->setState( 2 * pos3 + 0.2*Vector3s::Random());
F3->getOPtr()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized());
- std::string report = ceres_manager->solve(1);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
// Print results
WOLF_DEBUG("report: ", report);
@@ -745,14 +831,23 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
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();
+
// evaluate residuals
Vector3s res;
for (size_t i=0; i<cv123.size(); i++)
{
- cv123.at(i)->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ cv123.at(i)->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
@@ -804,13 +899,13 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
F1->addConstrainedBy(cd);
cd->setStatus(CTR_INACTIVE);
- std::string report = ceres_manager->solve(1);
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_DEBUG("DISTANCE CONSTRAINT INACTIVE: \n", report);
problem->print(1,0,1,0);
cd->setStatus(CTR_ACTIVE);
- report = ceres_manager->solve(1);
+ report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
// Print results
WOLF_DEBUG("DISTANCE CONSTRAINT ACTIVE: \n", report);
@@ -822,15 +917,23 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
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();
// evaluate residuals
Vector3s res;
for (size_t i=0; i<cv123.size(); i++)
{
- cv123.at(i)->operator ()(F1->getPPtr()->getPtr(), F1->getOPtr()->getPtr(),
- F2->getPPtr()->getPtr(), F2->getOPtr()->getPtr(),
- F3->getPPtr()->getPtr(), F3->getOPtr()->getPtr(),
- S ->getPPtr()->getPtr(), S ->getOPtr()->getPtr(),
+ cv123.at(i)->operator ()(F1_p.data(), F1_o.data(),
+ F2_p.data(), F2_o.data(),
+ F3_p.data(), F3_o.data(),
+ S_p. data(), S_o. data(),
res.data());
ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index f3b9e0cca1770a6b06d6f2a51c408d54d982a69b..68408277551644f6e04abd3a1bb3894ef0bb57af 100644
--- a/src/test/gtest_constraint_odom_3D.cpp
+++ b/src/test/gtest_constraint_odom_3D.cpp
@@ -43,7 +43,7 @@ FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeS
FrameBasePtr frm1 = problem->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
// Capture, feature and constraint from frm1 to frm0
-CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>(1, nullptr, delta, 7, 6, nullptr));
+CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
ConstraintOdom3DPtr ctr1 = std::static_pointer_cast<ConstraintOdom3D>(fea1->addConstraint(std::make_shared<ConstraintOdom3D>(fea1, frm0, nullptr))); // create and add
ConstraintBasePtr dummy = frm0->addConstrainedBy(ctr1);
diff --git a/src/test/gtest_constraint_pose_2D.cpp b/src/test/gtest_constraint_pose_2D.cpp
index c98de12b0f2dd05628ff905b8877e40daa80ba16..dd95c82d83a69a9e979e1dc3f5f5f26f9d6ea0cc 100644
--- a/src/test/gtest_constraint_pose_2D.cpp
+++ b/src/test/gtest_constraint_pose_2D.cpp
@@ -35,7 +35,7 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
// Capture, feature and constraint from frm1 to frm0
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose, 3, 3, nullptr));
+CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
ConstraintPose2DPtr ctr0 = std::static_pointer_cast<ConstraintPose2D>(fea0->addConstraint(std::make_shared<ConstraintPose2D>(fea0)));
diff --git a/src/test/gtest_constraint_pose_3D.cpp b/src/test/gtest_constraint_pose_3D.cpp
index 77604701ad57fc9851169a599822d10e6beb7c77..48fbedebd3b2a9a1f2932d8b18ef0979552d3ce3 100644
--- a/src/test/gtest_constraint_pose_3D.cpp
+++ b/src/test/gtest_constraint_pose_3D.cpp
@@ -42,7 +42,7 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
// Capture, feature and constraint
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose7, 7, 6, nullptr));
+CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0)));
diff --git a/src/test/gtest_feature_IMU.cpp b/src/test/gtest_feature_IMU.cpp
index 9f7e9c444d28dbc28efa29d8a7c01806731a9f26..b01de0dda778aa917c9b528aa1a880b0e4a1bfce 100644
--- a/src/test/gtest_feature_IMU.cpp
+++ b/src/test/gtest_feature_IMU.cpp
@@ -40,6 +40,10 @@ class FeatureIMU_test : public testing::Test
IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
+ prc_imu_params->max_time_span = 0.5;
+ prc_imu_params->max_buff_length = 10;
+ prc_imu_params->dist_traveled = 5;
+ prc_imu_params->angle_turned = 0.5;
processor_ptr_ = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
// Time and data variables
diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp
index 96d9edc8490face675cedfe654d18328ab1b579f..cc8e029033d1d8c331458aaeb0765401228dd3f1 100644
--- a/src/test/gtest_frame_base.cpp
+++ b/src/test/gtest_frame_base.cpp
@@ -73,10 +73,10 @@ TEST(FrameBase, LinksToTree)
T->addFrame(F1);
FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
T->addFrame(F2);
- CaptureMotionPtr C = make_shared<CaptureMotion>(1, S, Vector3s::Zero(), 3, 3, nullptr);
+ CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
F1->addCapture(C);
/// @todo link sensor & proccessor
- ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>();
+ ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
C->addFeature(f);
ConstraintOdom2DPtr c = make_shared<ConstraintOdom2D>(f, F2, p);
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index b8bb7388cad12f319a37e053d747df66ce0b63a5..2f3da744717fa0c9a8d99e6d163a09f0a16ea77c 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -193,12 +193,13 @@ TEST(Odom2D, VoteForKfAndSolve)
ProblemPtr problem = Problem::create("PO 2D");
SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->dist_traveled_th_ = 100;
- params->theta_traveled_th_ = 6.28;
- params->elapsed_time_th_ = 2.5*dt; // force KF at every third process()
- params->cov_det_th_ = 100;
- params->unmeasured_perturbation_std_ = 0.001;
- Matrix3s unmeasured_cov = params->unmeasured_perturbation_std_*params->unmeasured_perturbation_std_*Matrix3s::Identity();
+ params->voting_active = true;
+ params->dist_traveled = 100;
+ params->angle_turned = 6.28;
+ params->max_time_span = 2.5*dt; // force KF at every third process()
+ params->cov_det = 100;
+ params->unmeasured_perturbation_std = 0.00;
+ Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
@@ -262,6 +263,10 @@ TEST(Odom2D, VoteForKfAndSolve)
integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
}
+ WOLF_DEBUG("n: ", n, " t:", t);
+ WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
+ WOLF_DEBUG("test delta: ", integrated_delta .transpose());
+
ASSERT_POSE2D_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6);
@@ -314,12 +319,12 @@ TEST(Odom2D, KF_callback)
ProblemPtr problem = Problem::create("PO 2D");
SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->dist_traveled_th_ = 100;
- params->theta_traveled_th_ = 6.28;
- params->elapsed_time_th_ = 100;
- params->cov_det_th_ = 100;
- params->unmeasured_perturbation_std_ = 0.001;
- Matrix3s unmeasured_cov = params->unmeasured_perturbation_std_*params->unmeasured_perturbation_std_*Matrix3s::Identity();
+ params->dist_traveled = 100;
+ params->angle_turned = 6.28;
+ params->max_time_span = 100;
+ params->cov_det = 100;
+ params->unmeasured_perturbation_std = 0.001;
+ Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
processor_odom2d->setTimeTolerance(dt/2);
@@ -347,7 +352,7 @@ TEST(Odom2D, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
- CaptureMotionPtr capture = std::make_shared<CaptureMotion>(t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+ CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
for (int n=1; n<=N; n++)
{
diff --git a/src/test/gtest_odom_3D.cpp b/src/test/gtest_odom_3D.cpp
index 06dac12a7e45639a059a815f920d728b535aa07a..bc36ac8457f36b9747ff16e46e89bdc5e8e6f8cc 100644
--- a/src/test/gtest_odom_3D.cpp
+++ b/src/test/gtest_odom_3D.cpp
@@ -60,7 +60,7 @@ class ProcessorOdom3DTest : public ProcessorOdom3D
Scalar& dvar_min() {return min_disp_var_;}
Scalar& rvar_min() {return min_rot_var_;}
};
-ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D()
+ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D(std::make_shared<ProcessorParamsOdom3D>())
{
dvar_min() = 0.5;
rvar_min() = 0.25;
@@ -165,7 +165,7 @@ TEST(ProcessorOdom3D, Interpolate0) // basic test
* fin D = id
*/
- ProcessorOdom3D prc;
+ ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0);
@@ -205,7 +205,7 @@ TEST(ProcessorOdom3D, Interpolate0) // basic test
TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
{
- ProcessorOdom3D prc;
+ ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
/*
* We create several poses: origin, ref, int, second, final, as follows:
@@ -375,7 +375,7 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
{
- ProcessorOdom3D prc;
+ ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
/*
* We create several poses: origin, ref, int, second, final, as follows:
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index 420e9b271a16e70c41bd72df083f9ae6137cf5eb..2852598e3ba3d4b082b577e6a9f148e46cc86087 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -58,7 +58,7 @@ TEST(Problem, Processor)
P->addSensor(Sm);
// add motion processor
- ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>();
+ ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
Sm->addProcessor(Pm);
// check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers
@@ -79,7 +79,11 @@ TEST(Problem, Installers)
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10);
+ ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
+ params->time_tolerance = 0.1;
+ params->max_new_features = 5;
+ params->min_features_for_keyframe = 10;
+ ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
S->addProcessor(pt);
// check motion processor IS NOT set
@@ -217,7 +221,12 @@ TEST(Problem, StateBlocks)
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3);
ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3);
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10);
+ ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
+ params->time_tolerance = 0.1;
+ params->max_new_features = 5;
+ params->min_features_for_keyframe = 10;
+ ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
+
St->addProcessor(pt);
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
@@ -246,7 +255,13 @@ TEST(Problem, Covariances)
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10);
+
+ ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
+ params->time_tolerance = 0.1;
+ params->max_new_features = 5;
+ params->min_features_for_keyframe = 10;
+ ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
+
St->addProcessor(pt);
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index 38552b3334d5e0798f29aed417355e11d4ce94f7..b9751b42f939e5b6321f84134643273415145e40 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -51,7 +51,7 @@ class ProcessorIMUt : public testing::Test
problem = Problem::create("POV 3D");
Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+ ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
// Time and data variables
data = Vector6s::Zero();
@@ -83,13 +83,6 @@ TEST(ProcessorIMU_constructors, ALL)
{
using namespace wolf;
- //constructor without any argument
- ProcessorIMUPtr prc0 = std::make_shared<ProcessorIMU>();
- ProcessorParamsIMU params_default;
- ASSERT_EQ(prc0->getMaxTimeSpan(), params_default.max_time_span);
- ASSERT_EQ(prc0->getMaxBuffLength(), params_default.max_buff_length);
- ASSERT_EQ(prc0->getDistTraveled(), params_default.dist_traveled);
- ASSERT_EQ(prc0->getAngleTurned(), params_default.angle_turned);
//constructor with ProcessorIMUParamsPtr argument only
ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
@@ -98,7 +91,7 @@ TEST(ProcessorIMU_constructors, ALL)
param_ptr->dist_traveled = 2.0;
param_ptr->angle_turned = 2.0;
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(*param_ptr);
+ ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
@@ -110,6 +103,7 @@ TEST(ProcessorIMU_constructors, ALL)
Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+ ProcessorParamsIMU params_default;
ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
@@ -238,9 +232,10 @@ TEST_F(ProcessorIMUt, interpolate)
// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
-class P : wolf::ProcessorIMU
+class P : public wolf::ProcessorIMU
{
public:
+ P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
{
return ProcessorIMU::interpolate(ref, sec, ts);
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
index 9738bff9e33a605891e4b24fb3cac75b6084bfd0..23baef7b289909e86555b96e160f8823910523f9 100644
--- a/src/test/gtest_processor_base.cpp
+++ b/src/test/gtest_processor_base.cpp
@@ -42,7 +42,12 @@ TEST(ProcessorBase, KeyFrameCallback)
SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(dt/2, 5, 5);
+
+ ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
+ params->time_tolerance = dt/2;
+ params->max_new_features = 5;
+ params->min_features_for_keyframe = 5;
+ shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params);
problem->addSensor(sens_trk);
sens_trk->addProcessor(proc_trk);
diff --git a/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 47c79e0f137b1e2911575b0c74b640fb03e3c896..f114f3d8d506405f6b7d76a58d1ac52de04a45da 100644
--- a/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -18,7 +18,7 @@
// Dummy class so that it is no pur virtual
struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter
{
- DummyLoopCloser(const Params& _params) :
+ DummyLoopCloser(ParamsPtr _params) :
wolf::ProcessorFrameNearestNeighborFilter(_params) { }
bool confirmLoopClosure() override { return false; }
@@ -217,7 +217,7 @@ int main(int argc, char **argv)
processor_params_point2d->buffer_size_ = 0; // just exclude identical frameIDs
processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE;
- processor_ptr_point2d = std::make_shared<DummyLoopCloser>(*processor_params_point2d);
+ processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d);
processor_ptr_point2d->setName("processor2Dpoint");
sensor_ptr->addProcessor(processor_ptr_point2d);
@@ -226,7 +226,7 @@ int main(int argc, char **argv)
processor_params_ellipse2d->buffer_size_ = 0; // just exclude identical frameIDs
processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
- processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(*processor_params_ellipse2d);
+ processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d);
processor_ptr_ellipse2d->setName("processor2Dellipse");
sensor_ptr->addProcessor(processor_ptr_ellipse2d);
diff --git a/src/test/gtest_processor_motion.cpp b/src/test/gtest_processor_motion.cpp
index 6cd5e06450c0a99ab691c27081db20cd28f92cbd..77d2a55186ae6084f54e6832c71bc88067edc081 100644
--- a/src/test/gtest_processor_motion.cpp
+++ b/src/test/gtest_processor_motion.cpp
@@ -29,19 +29,21 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
Vector2s data;
Matrix2s data_cov;
+ ProcessorMotion_test() : ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()) { }
+
virtual void SetUp()
{
dt = 1.0;
problem = Problem::create("PO 2D");
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
- params->dist_traveled_th_ = 100;
- params->theta_traveled_th_ = 6.28;
- params->elapsed_time_th_ = 2.5*dt; // force KF at every third process()
- params->cov_det_th_ = 100;
- params->unmeasured_perturbation_std_ = 0.001;
+ params->dist_traveled = 100;
+ params->angle_turned = 6.28;
+ params->max_time_span = 2.5*dt; // force KF at every third process()
+ params->cov_det = 100;
+ params->unmeasured_perturbation_std = 0.001;
sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)));
processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params));
- capture = std::make_shared<CaptureMotion>(0.0, sensor, data, data_cov, 3, 3, nullptr);
+ capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
Vector3s x0; x0 << 0, 0, 0;
Matrix3s P0; P0.setIdentity();
diff --git a/src/test/gtest_processor_tracker_feature_trifocal.cpp b/src/test/gtest_processor_tracker_feature_trifocal.cpp
index 669c61566da78b8d09e71e73299eab81f6430586..71a8e7f469ceee695ae6da01fd6743949895ff2e 100644
--- a/src/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/src/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -3,7 +3,7 @@
#include "wolf.h"
#include "logging.h"
-#include "vision_utils/vision_utils.h"
+#include "vision_utils.h"
#include "processors/processor_tracker_feature_trifocal.h"
#include "processor_odom_3D.h"
@@ -12,7 +12,6 @@
using namespace Eigen;
using namespace wolf;
-using std::static_pointer_cast;
// Use the following in case you want to initialize tests with predefines variables or methods.
//class ProcessorTrackerFeatureTrifocal_class : public testing::Test{
@@ -23,7 +22,6 @@ using std::static_pointer_cast;
//};
-
//TEST(ProcessorTrackerFeatureTrifocal, Constructor)
//{
// std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Constructor is empty." << std::endl;
@@ -77,20 +75,26 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
ProblemPtr problem = Problem::create("PO 3D");
// Install tracker (sensor and processor)
- IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ IntrinsicsCameraPtr intr = make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
intr->width = 640;
intr->height = 480;
SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(),
intr);
- ProcessorParamsTrackerFeatureTrifocal params_trifocal;
- params_trifocal.name = "trifocal";
- params_trifocal.time_tolerance = dt/2;
- params_trifocal.max_new_features = 5;
- params_trifocal.min_features_for_keyframe = 5;
- params_trifocal.yaml_file_params_vision_utils = wolf_root + "/src/examples/ACTIVESEARCH.yaml";
-
- ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_trifocal);
+ ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
+ params_tracker_feature_trifocal->name = "trifocal";
+ params_tracker_feature_trifocal->pixel_noise_std = 1.0;
+ params_tracker_feature_trifocal->voting_active = true;
+ params_tracker_feature_trifocal->min_features_for_keyframe = 5;
+ params_tracker_feature_trifocal->time_tolerance = dt/2;
+ params_tracker_feature_trifocal->max_new_features = 5;
+ params_tracker_feature_trifocal->min_response_new_feature = 25;
+ params_tracker_feature_trifocal->n_cells_h = 10;
+ params_tracker_feature_trifocal->n_cells_v = 10;
+ params_tracker_feature_trifocal->max_euclidean_distance = 20;
+ params_tracker_feature_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/ACTIVESEARCH.yaml";
+
+ ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal);
proc_trk->configure(sens_trk);
problem->addSensor(sens_trk);
@@ -116,7 +120,8 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
CaptureOdom3DPtr capt_odo = make_shared<CaptureOdom3D>(t, sens_odo, Vector6s::Zero(), P);
// Track
- cv::Mat image(640,480, CV_32F);
+ cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols)
+ image *= 0; // TODO see if we want to use a real image
CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
proc_trk->process(capt_trk);
diff --git a/src/test/processor_IMU_UnitTester.cpp b/src/test/processor_IMU_UnitTester.cpp
index a37a50ece9b0084df96b0bd2f214289c33ff27e2..927b9dd91d15e14303ce21e6aa61bee5763d97d5 100644
--- a/src/test/processor_IMU_UnitTester.cpp
+++ b/src/test/processor_IMU_UnitTester.cpp
@@ -3,7 +3,7 @@
namespace wolf {
ProcessorIMU_UnitTester::ProcessorIMU_UnitTester() :
- ProcessorIMU(),
+ ProcessorIMU(std::make_shared<ProcessorParamsIMU>()),
Dq_out_(nullptr)
{}
diff --git a/src/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp b/src/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
index 9ca83b3d32366ac7b84b048a2c5d80aa1a474564..89fbef3d69dff80688dd234b0cec3742233b9220 100644
--- a/src/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
+++ b/src/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
@@ -25,9 +25,9 @@ public:
nb_.dist_traveled_th_ = 0.17;
nb_.theta_traveled_th_ = 0.3;
- nb_.cov_det_th_ = 0.4;
+ nb_.cov_det = 0.4;
nb_.elapsed_time_th_ = 1.5;
- nb_.unmeasured_perturbation_std_ = 1e-5;
+ nb_.unmeasured_perturbation_std = 1e-5;
}
const std::string path_to_io = "/tmp/";
@@ -58,9 +58,9 @@ TEST_F(WolfTestCerealSerializationProcessorParamsOdom2D,
ASSERT_EQ(nb_load.name, nb_.name) << full_path;
ASSERT_EQ(nb_load.dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
ASSERT_EQ(nb_load.theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.cov_det_th_, nb_.cov_det_th_) << full_path;
- ASSERT_EQ(nb_load.unmeasured_perturbation_std_,
- nb_.unmeasured_perturbation_std_) << full_path;
+ ASSERT_EQ(nb_load.cov_det, nb_.cov_det) << full_path;
+ ASSERT_EQ(nb_load.unmeasured_perturbation_std,
+ nb_.unmeasured_perturbation_std) << full_path;
/// Testing BasePtr
@@ -90,9 +90,9 @@ TEST_F(WolfTestCerealSerializationProcessorParamsOdom2D,
ASSERT_EQ(nb_cast->name, nb_.name) << full_path;
ASSERT_EQ(nb_cast->dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
ASSERT_EQ(nb_cast->theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->cov_det_th_, nb_.cov_det_th_) << full_path;
- ASSERT_EQ(nb_cast->unmeasured_perturbation_std_,
- nb_.unmeasured_perturbation_std_) << full_path;
+ ASSERT_EQ(nb_cast->cov_det, nb_.cov_det) << full_path;
+ ASSERT_EQ(nb_cast->unmeasured_perturbation_std,
+ nb_.unmeasured_perturbation_std) << full_path;
}
}
diff --git a/src/track_matrix.h b/src/track_matrix.h
index b31771403721ac874e789eda4b415ee350f50e49..7198b84e22dd7cbf109884e893d0af99163a0694 100644
--- a/src/track_matrix.h
+++ b/src/track_matrix.h
@@ -85,8 +85,8 @@ class TrackMatrix
void remove (CaptureBasePtr _cap);
size_t numTracks ();
size_t trackSize (size_t _track_id);
- Track track (size_t _track_id);
- Snapshot snapshot (CaptureBasePtr _capture);
+ Track track (size_t _track_id);
+ Snapshot snapshot (CaptureBasePtr _capture);
vector<FeatureBasePtr>
trackAsVector(size_t _track_id);
list<FeatureBasePtr>
@@ -104,7 +104,7 @@ class TrackMatrix
// Along track: maps of Feature pointers indexed by time stamp.
map<size_t, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
- // Across track: maps of Feature pointers indexed by Feature Id.
+ // Across track: maps of Feature pointers indexed by track_Id.
map<size_t, Snapshot > snapshots_; // map indexed by capture_Id of ( maps indexed by track_Id of ( features ) )
};
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index 4c633baaf32265c9e7ba33c79905de7dd768fb01..7de4b1587de6fd5dbe5aa4b86e00ac46d68dbbb2 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -9,12 +9,13 @@
#include "yaml_conversion.h"
// wolf
-#include "../processor_params_image.h"
#include "../factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
+#include "../processor_params_image.h"
+
namespace wolf
{
namespace
@@ -24,7 +25,7 @@ static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _fi
using std::string;
using YAML::Node;
- std::shared_ptr<ProcessorParamsImage> p = std::make_shared<ProcessorParamsImage>();
+ std::shared_ptr<ProcessorParamsTrackerFeatureImage> p = std::make_shared<ProcessorParamsTrackerFeatureImage>();
Node params = YAML::LoadFile(_filename_dot_yaml);
@@ -45,15 +46,15 @@ static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _fi
}
Node alg = params["algorithm"];
- p->algorithm.max_new_features = alg["maximum new features"].as<unsigned int>();
- p->algorithm.min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
- p->algorithm.min_response_for_new_features = alg["minimum response for new features"].as<float>();
- p->algorithm.time_tolerance= alg["time tolerance"].as<Scalar>();
- p->algorithm.distance= alg["distance"].as<Scalar>();
+ p->max_new_features = alg["maximum new features"].as<unsigned int>();
+ p->min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>();
+ p->min_response_for_new_features = alg["minimum response for new features"].as<float>();
+ p->time_tolerance= alg["time tolerance"].as<Scalar>();
+ p->distance= alg["distance"].as<Scalar>();
Node noi = params["noise"];
- p->noise.pixel_noise_std = noi["pixel noise std"].as<Scalar>();
- p->noise.pixel_noise_var = p->noise.pixel_noise_std * p->noise.pixel_noise_std;
+ p->pixel_noise_std = noi["pixel noise std"].as<Scalar>();
+ p->pixel_noise_var = p->pixel_noise_std * p->pixel_noise_std;
}
return p;
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index cb77aac131a61229e4d556426f6b19481f6cfea0..9e339d41aee83258fc9ecc2aaa0e9c76efdb8311 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -47,12 +47,17 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
std::string strNew = _filename_dot_yaml.substr (first,last-first);
params->yaml_file_params_vision_utils = _filename_dot_yaml.substr (first,last-first) + "/" + params->yaml_file_params_vision_utils;
- YAML::Node algorithm = config ["algorithm"];
- params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
- params->voting_active = algorithm["voting active"] .as<bool>();
+ YAML::Node algorithm = config ["algorithm"];
+ params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
+ params->voting_active = algorithm["voting active"] .as<bool>();
+ params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
+ params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
+ params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
+ params->n_cells_v = algorithm["grid vert cells"] .as<int>();
+ params->min_response_new_feature = algorithm["min response new features"] .as<int>();
+ params->max_euclidean_distance = algorithm["max euclidean distance"] .as<Scalar>();
+ params->min_track_length_for_constraint = algorithm["min track length for constraint"].as<int>();
- params->min_features_for_keyframe = algorithm["minimum features for keyframe"].as<unsigned int>();
- params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
YAML::Node noise = config["noise"];
params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>();