diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5765ab7eadbe19ab1479b8dc5c2df49aa7edde26..608f836ca8b9ddee82ab349a593831000eb62da6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,9 +64,18 @@ if(UNIX)
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+ OPTION(BUILD_DEMOS "Build Demos" OFF)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+ OPTION(BUILD_DOC "Build Documentation" OFF)
+ENDIF(NOT BUILD_DOC)
-#OPTION(BUILD_DOC "Build Documentation" OFF)
-OPTION(BUILD_TESTS "Build Unit tests" ON)
#############
## Testing ##
#############
@@ -97,10 +106,6 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-# option(BUILD_DEMOS "Build examples" OFF)
-set(BUILD_TESTS true)
-set(BUILD_DEMOS false)
-
# Does this has any other interest
# but for the examples ?
# yes, for the tests !
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 00592e5d6053dc0e75292b077529c19cbdc1a00d..a6b2e702003978c80a8c301513eccca05f28b1c5 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -22,6 +22,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing);
struct ProcessorParamsRangeBearing : public ProcessorParamsBase
{
// We do not need special parameters, but in case you need they should be defined here.
+ ProcessorParamsRangeBearing()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ProcessorParamsRangeBearing(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ //
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print();
+ }
};
using namespace Eigen;
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 5b80ff6b8f03ab7b9151252f29b99341212975b6..587259b40fd486015460a9d1e29c7573b589dcb9 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -29,6 +29,11 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std", "0.05");
noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std", "0.5");
}
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index d3e9e1ef41788c1c94dd3759fe06524ca4e1930a..d79f9e143eef67e982233b92df7a3cf289f9c100 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -375,6 +375,10 @@ struct ParamsBase
}
virtual ~ParamsBase() = default;
+ std::string print()
+ {
+ return "";
+ }
};
} // namespace wolf
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 412d3b3289d9f2ef3a00151c71d2da66dde94649..5808bdadf0575a26b874791e565242b810c5e2a2 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -122,6 +122,7 @@ struct ProcessorParamsBase : public ParamsBase
ParamsBase(_unique_name, _server)
{
voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
+ voting_aux_active = _server.getParam<bool>(_unique_name + "/voting_aux_active", "false");
time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0");
}
@@ -134,6 +135,12 @@ struct ProcessorParamsBase : public ParamsBase
/// a particular Capture of this processor to allow assigning
/// this Capture to the Keyframe.
Scalar time_tolerance = Scalar(0);
+ std::string print()
+ {
+ return ParamsBase::print() + "\n" + "voting_active: " + std::to_string(voting_active) + "\n"
+ + "voting_aux_active: " + std::to_string(voting_aux_active) + "\n"
+ + "time_tolerance: " + std::to_string(time_tolerance) + "\n";
+ }
};
//class ProcessorBase
diff --git a/include/core/processor/processor_capture_holder.h b/include/core/processor/processor_capture_holder.h
index 1746fd548a5a7dfb0e5f9cdaa860527b720992ae..46fc538d9a05f53181f7d3d4d5e46a7e0e6a4dc4 100644
--- a/include/core/processor/processor_capture_holder.h
+++ b/include/core/processor/processor_capture_holder.h
@@ -24,6 +24,16 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
{
Scalar buffer_size = 30;
+ ProcessorParamsCaptureHolder() = default;
+ ProcessorParamsCaptureHolder(std::string _unique_name, const wolf::paramsServer & _server):
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ buffer_size = _server.getParam<Scalar>(_unique_name + "/buffer_size");
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print() + "buffer_size: " + std::to_string(buffer_size) + "\n";
+ }
};
/**
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 2b5f1a4942f662d78b3fd97acb9714f8a00ff9ee..60fbf8d295e0dd519e4d5a69cf4dc52e18349811 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -25,6 +25,10 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
{
unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print() + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ }
};
/**
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 0c6f2175ed28613b2497c9da94f9bdbb675a7973..56269e2041bc18e039dcdd6e64e228dc6aa35cc6 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -39,6 +39,15 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5");
unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4");
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print() + "max_time_span: " + std::to_string(max_time_span) + "\n"
+ + "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
+ + "angle_turned: " +std::to_string(angle_turned) + "\n"
+ + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ }
+
};
/** \brief class for Motion processors
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 90d47452d1334d731b39c6154edfef8400573b0a..fdcf820856bb061e445273a72334b20698b9564b 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -26,9 +26,12 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server):
ProcessorParamsMotion(_unique_name, _server)
{
- cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001");
+ cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n";
+ }
};
class ProcessorOdom2D : public ProcessorMotion
{
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 38683c57f04c5b6908bac19e26b5cc9ee3353e49..b5fc2dedf099e74bfbd64e7b28ca313cdd1bd412 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -27,6 +27,10 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion
{
//
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print();
+ }
};
WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index efcf4d55beab706794c8f869e727688e1a653169..17de27c05b0e210b12f46a3d43ad89d6ab8510ef 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -27,6 +27,12 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1");
}
+ std::string print()
+ {
+ return ProcessorParamsBase::print() + "\n" + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
+ + "max_new_features: " + std::to_string(max_new_features) + "\n";
+ }
+
};
WOLF_PTR_TYPEDEFS(ProcessorTracker);
@@ -202,6 +208,10 @@ class ProcessorTracker : public ProcessorBase
FeatureBasePtrList& getNewFeaturesListLast();
+ std::string print(){
+ return this->params_tracker_->print();
+ }
+
protected:
void computeProcessingStep();
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index e5333b510e3dd5ac65392826ee8d966ddcf74231..99a227a4010dffb1bdcddc18c390b94e5dafc2e9 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -23,8 +23,12 @@ namespace wolf {
*/
struct IntrinsicsBase: public ParamsBase
{
- virtual ~IntrinsicsBase() = default;
+ virtual ~IntrinsicsBase() = default;
using ParamsBase::ParamsBase;
+ std::string print()
+ {
+ return "";
+ }
};
class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index c1cbd7c382c58244e0e6a12f31b27d472b3507de..114be0e4d3342ed4936be3d0d9a6938e0735f898 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -61,6 +61,24 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01");
}
virtual ~IntrinsicsDiffDrive() = default;
+ std::string print()
+ {
+ std::string model_string;
+
+ if(model_ == DiffDriveModel::Two_Factor_Model) model_string = "Two Factor Model";
+ else if(model_ == DiffDriveModel::Three_Factor_Model) model_string = "Three Factor Model";
+ else if(model_ == DiffDriveModel::Five_Factor_Model) model_string = "Five Factor Model";
+
+ return "\n" + IntrinsicsBase::print() + "left_radius: " + std::to_string(left_radius_) + "\n"
+ + "right_radius: " + std::to_string(right_radius_) + "\n"
+ + "separation_: " + std::to_string(separation_) + "\n"
+ + "model_string: " + model_string + "\n"
+ + "factors_: " + converter<std::string>::convert(factors_) + "\n"
+ + "left_resolution_: " + std::to_string(left_resolution_) + "\n"
+ + "right_resolution_: " + std::to_string(right_resolution_) + "\n"
+ + "left_gain_: " + std::to_string(left_gain_) + "\n"
+ + "right_gain_: " + std::to_string(right_gain_) + "\n";
+ }
};
typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h
index 02218e8698790d207e61594befd403bca916862a..94bbd03d3778221d1a7a2a0d51935f993e9ddbb7 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2D.h
@@ -25,6 +25,11 @@ struct IntrinsicsOdom2D : public IntrinsicsBase
k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
}
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorOdom2D);
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
index dffaf433ebd1ff29a258aec53e5478874b51f7c7..827e4e2cf59d4d660a7d72ea5811f6a66d27f775 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3D.h
@@ -36,6 +36,14 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
}
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
+ + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
+ }
virtual ~IntrinsicsOdom3D() = default;
};
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index ffb955d456f515aa0871a54eea4de84df94af187..36d59e759018377bb81266932a57d7a5d827b318 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -52,8 +52,8 @@ namespace utils{
* @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
*/
static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){
- std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]");
- std::regex rgxStatic("\\[((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]");
+ std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]*\\.)?[0-9]+,?)+)\\]");
+ std::regex rgxStatic("\\[((?:(?:[0-9]*\\.)?[0-9]+,?)+)\\]");
std::smatch matches;
std::smatch matchesStatic;
std::array<std::string,2> values = {{"[]","[]"}};
@@ -97,7 +97,7 @@ namespace wolf{
template<typename T>
struct converter{
static T convert(std::string val){
- throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: "+ val);
+ throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: " + val);
}
};
template<typename A>
@@ -241,7 +241,7 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo
auto values = converter<std::vector<_Scalar>>::convert(splittedValues[1]);
Matrix m = Matrix();
if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic) {
- if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing Eigen Matrix. Missing dimensions. String provided: " + val);
+ if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing a dynamic Eigen Matrix. Missing dimensions. String provided: " + val);
m.resize(dimensions[0],dimensions[1]);
}else if(_Rows == Eigen::Dynamic){
int nrows = (int) values.size() / _Cols;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d590892a8d7137552074fa01e61f69feb78c7431..b607d1b356e3772439e05bc62885f3bcd83a2de6 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -75,9 +75,6 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
}
ProblemPtr Problem::autoSetup(paramsServer &_server)
{
- // parserYAML parser = parserYAML(_yaml_file, _root_path);
- // parser.parse();
- // paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
std::string frame_structure = _server.getParam<std::string>("problem/frame structure", "PO");
int dim = _server.getParam<int>("problem/dimension", "2");
auto p = Problem::create(frame_structure, dim);
@@ -86,24 +83,30 @@ ProblemPtr Problem::autoSetup(paramsServer &_server)
// cout << "-----------------------------------" << endl;
auto loaders = std::vector<Loader*>();
for(auto it : _server.getParam<std::vector<std::string>>("files")) {
- std::cout << "LOADING " << it << std::endl;
+ WOLF_TRACE("Loading file " + it)
auto l = new LoaderRaw(it);
l->load();
loaders.push_back(l);
}
- //TODO: To be fixed. This prior should be set in here, but now it is set externally.
- // setPrior(Eigen::Vector3s::Zero(), 0.1*Eigen::Matrix3s::Identity(), TimeStamp(), Scalar(0.1));
+ WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension {" + std::to_string(dim) + "}");
+ Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior state");
+ Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior cov");
+ Scalar time_tolerance = _server.getParam<Scalar>("problem/time tolerance");
+ auto ts = TimeStamp();
+ WOLF_TRACE("prior timestamp:\n",ts);
+ WOLF_TRACE("prior state:\n", prior_state.transpose());
+ WOLF_TRACE("prior covariance:\n", prior_cov);
+ WOLF_TRACE("prior time tolerance:\n", time_tolerance);
+ p->setPrior(prior_state, prior_cov, ts, time_tolerance);
auto sensorMap = std::map<std::string, SensorBasePtr>();
auto procesorMap = std::map<std::string, ProcessorBasePtr>();
- for(auto s : _server.getSensors()){
- // cout << s._name << " " << s._type << endl;
- sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, _server)));
- }
- for(auto s : _server.getProcessors()){
- // cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
+ for(auto s : _server.getSensors()){
+ sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, _server)));
+ }
+ for(auto s : _server.getProcessors()){
procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, _server)));
- }
- return p;
+ }
+ return p;
}
Problem::~Problem()
@@ -927,7 +930,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
for (auto C : F->getCaptureList())
{
cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
-
+
if(C->getSensor() != nullptr)
{
cout << " -> S" << C->getSensor()->id();
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 5735441cb55404882bf9cd32c712a843daa61324..37f840a505c86754b6e6a4d865a45e441d6feac1 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -107,8 +107,8 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
{
- assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
- assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
+ assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
+ assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
_buffer_part_before_ts.setCalibrationPreint(calib_preint_);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0b646d71b9e670d8d81725b2a8e80b1f02187da8..e3a115798c5e01cd3b3385aab38b1f499957f253 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -96,7 +96,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
-
case RUNNING_WITHOUT_PACK :
case RUNNING_WITH_PACK_ON_ORIGIN :
break;
@@ -658,6 +657,10 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+ // ignore "future" KF to avoid MotionBuffer::split() error
+ if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_)
+ pack = nullptr;
+
if (pack)
{
if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))