diff --git a/.gitignore b/.gitignore index d683ba3c89247c8f1af43ea1cf25a7975f220dd8..6c5bf11964b51ece53f21d2cc93e962a6c9e461b 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,7 @@ src/CMakeCache.txt src/CMakeFiles/cmake.check_cache src/examples/map_apriltag_save.yaml + +\.vscode/ +build_release/ + diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake index 82bb7cf0a5523302eb3d6c7fd5064e382a6c59f5..c9321c445315da6e354b1f8423b7368745c14816 100644 --- a/cmake_modules/wolfConfig.cmake +++ b/cmake_modules/wolfConfig.cmake @@ -156,7 +156,6 @@ list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3) # match and reject with an explanation below. find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET) - # Flag set with currently found Eigen version. set(EIGEN_VERSION @EIGEN_VERSION@) if (EIGEN3_FOUND) diff --git a/hello_plugin/CMakeLists.txt b/hello_plugin/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..8df392f87658baa3beb7af2a98055c8a88dde949 --- /dev/null +++ b/hello_plugin/CMakeLists.txt @@ -0,0 +1,16 @@ +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) +# INCLUDE_DIRECTORIES(/home/jcasals/workspace/wolf/src) +# link_directories(/home/jcasals/workspace/wolf/lib) + +ADD_EXECUTABLE(hello_plugin hello_plugin.cpp) +ADD_EXECUTABLE(params_autoconf params_autoconf.cpp) +# target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES}) +# target_link_libraries(params_autoconf class_loader boost_system console_bridge wolf yaml-cpp ) +target_link_libraries(hello_plugin wolf yaml-cpp ${CERES_LIBRARIES} dl) +target_link_libraries(params_autoconf wolf yaml-cpp dl) + +# These lines always at the end +SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE ) +SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE ) + + diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..54a95eb5ea5f196129c8a46d916ad325bb9fd022 --- /dev/null +++ b/hello_plugin/hello_plugin.cpp @@ -0,0 +1,182 @@ +/* + * hello_plugin.cpp + * + * Created on: Nov 12, 2018 + * Author: jcasals + */ +#include "base/sensor/sensor_base.h" +#include "base/common/wolf.h" +// #include "sensor_odom_2D.cpp" +#include <yaml-cpp/yaml.h> +#include "base/yaml/parser_yaml.hpp" +#include "base/utils/params_server.hpp" + +#include "../hello_wolf/capture_range_bearing.h" +#include "../hello_wolf/feature_range_bearing.h" +#include "../hello_wolf/factor_range_bearing.h" +#include "../hello_wolf/landmark_point_2D.h" +#include "base/utils/loader.hpp" +#include "base/processor/processor_odom_2D.h" + +#include "base/solver/solver_factory.h" +#include "base/ceres_wrapper/ceres_manager.h" + +using namespace std; +using namespace wolf; +using namespace Eigen; + +int main(int argc, char** argv) { + string file = ""; + if(argc > 1) file = argv[1]; + parserYAML parser = parserYAML(file); + parser.parse(); + paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); + cout << "PRINTING SERVER MAP" << endl; + server.print(); + cout << "-----------------------------------" << endl; + /** + It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get + a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because + the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is. + **/ + // vector<ClassLoader*> class_loaders = vector<ClassLoader*>(); + // for(auto it : parser.getFiles()) { + // auto c = new ClassLoader(it); + // class_loaders.push_back(c); + // } + auto loaders = vector<Loader*>(); + for(auto it : parser.getFiles()) { + auto l = new LoaderRaw(it); + loaders.push_back(l); + } + ProblemPtr problem = Problem::create("PO", 2); + auto sensorMap = map<string, SensorBasePtr>(); + auto procesorMap = map<string, ProcessorBasePtr>(); + for(auto s : server.getSensors()){ + cout << s._name << " " << s._type << endl; + sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server))); + } + for(auto s : server.getProcessors()){ + cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; + procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server))); + } + + problem->print(4,1,1,1); + Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn. + Matrix2s motion_cov = 0.1 * Matrix2s::Identity(); + + // landmark observations data + VectorXi ids; + VectorXs ranges, bearings; + + + // SET OF EVENTS ======================================================= + std::cout << std::endl; + WOLF_TRACE("======== BUILD PROBLEM ======="); + + // ceres::Solver::Options options; + // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations + // CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); + auto ceres = SolverFactory::get().create("Solver", problem, server); + // We'll do 3 steps of motion and landmark observations. + + // STEP 1 -------------------------------------------------------------- + + // initialize + TimeStamp t(0.0); // t : 0.0 + Vector3s x(0,0,0); + Matrix3s P = Matrix3s::Identity() * 0.1; + problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0) + auto sensor_rb = sensorMap.find("rb")->second; + // observe lmks + ids.resize(1); ranges.resize(1); bearings.resize(1); + ids << 1; // will observe Lmk 1 + ranges << 1.0; // see drawing + bearings << M_PI/2; + CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); + sensor_rb ->process(cap_rb); // L1 : (1,2) + + // STEP 2 -------------------------------------------------------------- + t += 1.0; // t : 1.0 + + // motion + auto sensor_odometry = sensorMap.find("odom")->second; + CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odometry, motion_data, motion_cov); + sensor_odometry ->process(cap_motion); // KF2 : (1,0,0) + + // observe lmks + ids.resize(2); ranges.resize(2); bearings.resize(2); + ids << 1, 2; // will observe Lmks 1 and 2 + ranges << sqrt(2.0), 1.0; // see drawing + bearings << 3*M_PI/4, M_PI/2; + cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); + sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2) + + // STEP 3 -------------------------------------------------------------- + t += 1.0; // t : 2.0 + + // motion + cap_motion ->setTimeStamp(t); + sensor_odometry ->process(cap_motion); // KF3 : (2,0,0) + // observe lmks + ids.resize(2); ranges.resize(2); bearings.resize(2); + ids << 2, 3; // will observe Lmks 2 and 3 + ranges << sqrt(2.0), 1.0; // see drawing + bearings << 3*M_PI/4, M_PI/2; + cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); + sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2) + problem->print(1,0,1,0); + + + // SOLVE ================================================================ + + // SOLVE with exact initial guess + WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======") + std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); + WOLF_TRACE(report); // should show a very low iteration number (possibly 1) + problem->print(1,0,1,0); + + // PERTURB initial guess + WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") + for (auto sen : problem->getHardware()->getSensorList()) + for (auto sb : sen->getStateBlockVec()) + if (sb && !sb->isFixed()) + sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + for (auto kf : problem->getTrajectory()->getFrameList()) + if (kf->isKey()) + for (auto sb : kf->getStateBlockVec()) + if (sb && !sb->isFixed()) + sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + for (auto lmk : problem->getMap()->getLandmarkList()) + for (auto sb : lmk->getStateBlockVec()) + if (sb && !sb->isFixed()) + sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + problem->print(1,0,1,0); + + // SOLVE again + WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") + report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); + WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!) + problem->print(1,0,1,0); + + // GET COVARIANCES of all states + WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") + ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + for (auto kf : problem->getTrajectory()->getFrameList()){ + if (kf->isKey()) + { + Eigen::MatrixXs cov; + WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov)); + } + for (auto lmk : problem->getMap()->getLandmarkList()) { + Eigen::MatrixXs cov; + WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov)); + } + } + std::cout << std::endl; + + WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======") + problem->print(4,1,1,1); + + return 0; +} diff --git a/hello_plugin/params.yaml b/hello_plugin/params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..14c4a1fa4753080997625511d5aa092611ce0101 --- /dev/null +++ b/hello_plugin/params.yaml @@ -0,0 +1,30 @@ +config: + sensors: + - + type: "ODOM 2D" + name: "odom" + intrinsic: + k_disp_to_disp: 0.1 + k_rot_to_rot: 0.1 + extrinsic: + pos: [1,2,3] + - + type: "RANGE BEARING" + name: "rb" + processors: + - + type: "ODOM 2D" + name: "processor1" + sensor name: "odom" + - + type: "RANGE BEARING" + name: "rb_processor" + sensor name: "rb" + - + type: "ODOM 2D" + name: "my_proc_test" + sensor name: "odom" + follow: "../hello_plugin/params_conf.yaml" #Config continues in this file +files: + - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odom.so" + - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file diff --git a/hello_plugin/params_autoconf.cpp b/hello_plugin/params_autoconf.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ac95d6b655d91b25a0d31152bd77bffe369e010a --- /dev/null +++ b/hello_plugin/params_autoconf.cpp @@ -0,0 +1,71 @@ +/* + * params_autoconf.cpp + * + * Created on: Feb 15, 2019 + * Author: jcasals + */ +#include "base/sensor/sensor_base.h" +#include "base/common/wolf.h" +// #include "sensor_odom_2D.cpp" +#include <yaml-cpp/yaml.h> +#include "base/yaml/parser_yaml.hpp" +#include "base/utils/params_server.hpp" + +#include "../hello_wolf/capture_range_bearing.h" +#include "../hello_wolf/feature_range_bearing.h" +#include "../hello_wolf/factor_range_bearing.h" +#include "../hello_wolf/landmark_point_2D.h" +#include "base/utils/loader.hpp" +#include "base/processor/processor_odom_2D.h" + +#include "base/solver/solver_factory.h" +#include "base/ceres_wrapper/ceres_manager.h" + +using namespace std; +using namespace wolf; +using namespace Eigen; + +int main(int argc, char** argv) { + string file = ""; + if(argc > 1) file = argv[1]; + parserYAML parser = parserYAML(file); + parser.parse(); + paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); + cout << "PRINTING SERVER MAP" << endl; + server.print(); + cout << "-----------------------------------" << endl; + /** + It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get + a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because + the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is. + **/ + auto loaders = vector<Loader*>(); + for(auto it : parser.getFiles()) { + auto l = new LoaderRaw(it); + l->load(); + loaders.push_back(l); + } + ProblemPtr problem = Problem::create("PO", 2); + auto sensorMap = map<string, SensorBasePtr>(); + auto procesorMap = map<string, ProcessorBasePtr>(); + for(auto s : server.getSensors()){ + cout << s._name << " " << s._type << endl; + sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server))); + } + for(auto s : server.getProcessors()){ + cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; + procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server))); + } + auto prc = ProcessorParamsOdom2D("my_proc_test", server); + + std::cout << "prc.cov_det " << prc.cov_det << std::endl; + std::cout << "prc.unmeasured_perturbation_std " << prc.unmeasured_perturbation_std << std::endl; + std::cout << "prc.angle_turned " << prc.angle_turned << std::endl; + std::cout << "prc.dist_traveled " << prc.dist_traveled << std::endl; + std::cout << "prc.max_buff_length " << prc.max_buff_length << std::endl; + std::cout << "prc.max_time_span " << prc.max_time_span << std::endl; + std::cout << "prc.time_tolerance " << prc.time_tolerance << std::endl; + std::cout << "prc.voting_active " << prc.voting_active << std::endl; + + return 0; +} diff --git a/hello_plugin/params_conf.yaml b/hello_plugin/params_conf.yaml new file mode 100644 index 0000000000000000000000000000000000000000..0d80a5802f5da772f59c29adf12257c8f79663d3 --- /dev/null +++ b/hello_plugin/params_conf.yaml @@ -0,0 +1,8 @@ +cov_det: 100 +unmeasured_perturbation_std: 100 +angle_turned: 100 +dist_traveled: 100 +max_buff_length: 100 +max_time_span: 100 +time_tolerance: 100 +voting_active: false \ No newline at end of file diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt index df4b2b136f5215ead121440939fedff4e428af32..20309f55ccc7ee673ecb67fc7f5de039369d9282 100644 --- a/hello_wolf/CMakeLists.txt +++ b/hello_wolf/CMakeLists.txt @@ -13,22 +13,21 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} ) SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} -# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp +# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp ) ADD_EXECUTABLE(hello_wolf hello_wolf.cpp) TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME}) - +add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp) +TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME}) +add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp) +TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME}) - - # These lines always at the end SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE ) SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE ) - - diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp index feeb6ac129e7ee004957b711ddf673f9c09722da..174945801fa1795d80ad723e054216190392d62c 100644 --- a/hello_wolf/hello_wolf.cpp +++ b/hello_wolf/hello_wolf.cpp @@ -12,10 +12,10 @@ * \author: jsola */ -#include "core/common/wolf.h" +#include "base/common/wolf.h" -#include "core/sensor/sensor_odom_2D.h" -#include "core/processor/processor_odom_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" #include "sensor_range_bearing.h" #include "processor_range_bearing.h" #include "capture_range_bearing.h" @@ -23,7 +23,7 @@ #include "factor_range_bearing.h" #include "landmark_point_2D.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/ceres_wrapper/ceres_manager.h" int main() { @@ -104,7 +104,7 @@ int main() using namespace wolf; // Wolf problem and solver - ProblemPtr problem = Problem::create("PO 2D"); + ProblemPtr problem = Problem::create("PO", 2); ceres::Solver::Options options; options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); @@ -225,7 +225,7 @@ int main() if (sb && !sb->isFixed()) sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! for (auto kf : problem->getTrajectory()->getFrameList()) - if (kf->isKey()) + if (kf->isKeyOrAux()) for (auto sb : kf->getStateBlockVec()) if (sb && !sb->isFixed()) sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! @@ -245,7 +245,7 @@ int main() WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); for (auto kf : problem->getTrajectory()->getFrameList()) - if (kf->isKey()) + if (kf->isKeyOrAux()) { Eigen::MatrixXs cov; kf->getCovariance(cov); diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index f12702ba9ddc09c70d0195d8e9a78410d350c3e3..d426a5a5b084462b2c04c47db6847eb30c74fd3f 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -47,7 +47,8 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture); - kf->addCapture(capture_rb); + // kf->addCapture(capture_rb); + capture_rb->link(kf); // 3. explore all observations in the capture for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++) @@ -70,9 +71,10 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) { // new landmark: // - create landmark - lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing)); + // lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing)); + lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing))); WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose()); - getProblem()->getMap()->addLandmark(lmk); + // getProblem()->getMap()->addLandmark(lmk); // - add to known landmarks known_lmks.emplace(id, lmk); } @@ -81,17 +83,23 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) Vector2s rb(range,bearing); auto ftr = std::make_shared<FeatureRangeBearing>(rb, getSensor()->getNoiseCov()); - capture_rb->addFeature(ftr); + // capture_rb->addFeature(ftr); + FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov()); // 6. create factor auto prc = shared_from_this(); - auto ctr = std::make_shared<FactorRangeBearing>(capture_rb, - lmk, - prc, - false, - FAC_ACTIVE); - ftr->addFactor(ctr); - lmk->addConstrainedBy(ctr); + // auto ctr = std::make_shared<FactorRangeBearing>(capture_rb, + // lmk, + // prc, + // false, + // FAC_ACTIVE); + auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, + lmk, + prc, + false, + FAC_ACTIVE); + // ftr->addFactor(ctr); + // lmk->addConstrainedBy(ctr); } } @@ -110,6 +118,24 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, return prc; } +ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name, + const paramsServer& _server, + const SensorBasePtr _sensor_ptr) +{ + SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr); + ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(); + params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false"); + params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.01"); + + // construct processor + ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); + + // setup processor + prc->setName(_unique_name); + + return prc; +} + Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const { return polar(toSensor(lmk_w)); @@ -167,4 +193,9 @@ namespace wolf { WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing) } // namespace wolf +#include "base/processor/autoconf_processor_factory.h" +namespace wolf +{ +WOLF_REGISTER_PROCESSOR_AUTO("RANGE BEARING", ProcessorRangeBearing) +} // namespace wolf diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 0dd2b7951fb63f0b1e487603f95ae2ff79eaaf2f..30b2bd8654a8bc8117f761e492f889fb44918a4a 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -40,6 +40,9 @@ class ProcessorRangeBearing : public ProcessorBase static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); + static ProcessorBasePtr createAutoConf(const std::string& _unique_name, + const paramsServer& _server, + const SensorBasePtr _sensor_ptr = nullptr); protected: // Implementation of pure virtuals from ProcessorBase diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp index dcdd23f1a15fdcbd498af9adf2d5a3112e91a9ee..571184f383832b931f3602a07c9f498a35480a71 100644 --- a/hello_wolf/sensor_range_bearing.cpp +++ b/hello_wolf/sensor_range_bearing.cpp @@ -6,7 +6,7 @@ */ #include "sensor_range_bearing.h" -#include "core/state_angle.h" +#include "base/state_block/state_angle.h" namespace wolf { @@ -42,12 +42,26 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // return sensor; } +SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, // + const paramsServer& _server) +{ + Eigen::Vector2s noise_std(0.1,1.0); + SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std); + sensor->setName(_unique_name); + // sensor->node_name_ = _unique_name; + return sensor; +} + } /* namespace wolf */ // Register in the SensorFactory -#include "core/sensor/sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing) } // namespace wolf - +#include "base/sensor/autoconf_sensor_factory.h" +namespace wolf +{ +WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing) +} // namespace wolf diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 5b60f0bb28ce8ba4a75f00f07a6ef735b770a252..c57c7761e55987ec3c69520971a478944f2c0eb1 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -8,7 +8,8 @@ #ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_ #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_ -#include "core/sensor/sensor_base.h" +#include "base/sensor/sensor_base.h" +#include "base/utils/params_server.hpp" namespace wolf { @@ -18,6 +19,16 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase { Scalar noise_range_metres_std = 0.05; Scalar noise_bearing_degrees_std = 0.5; + IntrinsicsRangeBearing() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + IntrinsicsRangeBearing(std::string _unique_name, const paramsServer& _server): + IntrinsicsBase(_unique_name, _server) + { + 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"); + } }; WOLF_PTR_TYPEDEFS(SensorRangeBearing) @@ -32,6 +43,8 @@ class SensorRangeBearing : public SensorBase static SensorBasePtr create(const std::string& _unique_name, // const Eigen::VectorXs& _extrinsics, // const IntrinsicsBasePtr _intrinsics); + static SensorBasePtr createAutoConf(const std::string& _unique_name, // + const paramsServer& _server); }; } /* namespace wolf */ diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h index a398efa6ff472c0e4ec65a5732f571c6bc1fed6e..1385e1af088678e853c0ab0ecb4f5d6a34f103a0 100644 --- a/serialization/cereal/serialization_local_parametrization_base.h +++ b/serialization/cereal/serialization_local_parametrization_base.h @@ -1,7 +1,7 @@ #ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ #define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ -#include "core/local_parametrization_base.h" +#include "base/state_block/local_parametrization_base.h" #include <cereal/cereal.hpp> #include <cereal/types/polymorphic.hpp> diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 836caa3d1c337a37eb8680871139251a371353a2..6ee86ed12165a0cc0bc78c93822a0b5a259eddb0 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -120,6 +120,11 @@ IF(vision_utils_FOUND) ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp) TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME}) + IF (APRILTAG_LIBRARY) + ADD_EXECUTABLE(test_apriltag test_apriltag.cpp) + TARGET_LINK_LIBRARIES(test_apriltag ${PROJECT_NAME}) + ENDIF(APRILTAG_LIBRARY) + ENDIF(Ceres_FOUND) # Testing OpenCV functions for projection of points diff --git a/src/examples/camera_Dinesh_LAAS_params.yaml b/src/examples/camera_Dinesh_LAAS_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..739505d12e8a2dd224b99220ee024ddc8efd9508 --- /dev/null +++ b/src/examples/camera_Dinesh_LAAS_params.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: mono +camera_matrix: + rows: 3 + cols: 3 + data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.416913, 0.264210, -0.000221, -0.000326, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml b/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml new file mode 100644 index 0000000000000000000000000000000000000000..dd2f6433f2b60c21b68ebf70b595af981550923c --- /dev/null +++ b/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: mono +camera_matrix: + rows: 3 + cols: 3 + data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.416913, 0.264210, 0, 0, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/examples/camera_logitech_c300_640_480.yaml b/src/examples/camera_logitech_c300_640_480.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5f8a1899b71df3721e6f9722d39bd8e09e34509a --- /dev/null +++ b/src/examples/camera_logitech_c300_640_480.yaml @@ -0,0 +1,22 @@ +image_width: 640 +image_height: 480 +#camera_name: narrow_stereo +camera_name: camera +camera_matrix: + rows: 3 + cols: 3 + data: [711.687376, 0.000000, 323.306816, 0.000000, 710.933260, 232.005822, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.067204, -0.141639, 0, 0, 0] +# data: [0.067204, -0.141639, 0.004462, -0.000636, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [718.941772, 0.000000, 323.016804, 0.000000, 0.000000, 717.174622, 233.475721, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/examples/camera_params_canonical.yaml b/src/examples/camera_params_canonical.yaml index 370b662c9b958d0c4ab528df8ab793567141f685..2508a0bec574125ae9dea63e558528b7211079d1 100644 --- a/src/examples/camera_params_canonical.yaml +++ b/src/examples/camera_params_canonical.yaml @@ -17,4 +17,4 @@ rectification_matrix: projection_matrix: rows: 3 cols: 4 - data: [1, 0, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0] \ No newline at end of file + data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/src/examples/map_apriltag_1.yaml b/src/examples/map_apriltag_1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d0c6a90707ddd2d15a2f921f244f085ecb337e6f --- /dev/null +++ b/src/examples/map_apriltag_1.yaml @@ -0,0 +1,42 @@ +map name: "Example of map of Apriltag landmarks" + +nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error. + +landmarks: + + - id : 1 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 1 + tag width: 0.1 + position: [0, 0, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 3 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 3 + tag width: 0.1 + position: [1, 1, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 5 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 5 + tag width: 0.1 + position: [1, 0, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 2 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 2 + tag width: 0.1 + position: [0, 1, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + diff --git a/src/examples/maps/map_apriltag_logitech_1234.yaml b/src/examples/maps/map_apriltag_logitech_1234.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f313d1a11a3f2b4fa239f710cbbea7372747677d --- /dev/null +++ b/src/examples/maps/map_apriltag_logitech_1234.yaml @@ -0,0 +1,46 @@ +map name: "4 tags printed on a A4 sheet vertical recorded at IRI with logitech webcam." + +nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error. + +###################### +# World frame is considered to be the top left corner of tag id 0. Reference frame is corherent with a camera +# looking straight at the sheet with RBF convention. +###################### +landmarks: + + - id : 0 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 0 + tag width: 0.055 + position: [0.0225, 0.0225, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 1 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 1 + tag width: 0.055 + position: [0.1525, 0.0225, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 2 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 2 + tag width: 0.055 + position: [0.0225, 0.2125, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 3 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 3 + tag width: 0.055 + position: [0.1525, 0.2125, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + diff --git a/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml index 070f92f9a45e1ff8be6592fbcdca374a26b92b8d..8e8a6c8cd0b75c0366a7aa83db4f3d54e4687fa6 100644 --- a/src/examples/processor_imu.yaml +++ b/src/examples/processor_imu.yaml @@ -1,6 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails -time tolerance: 0.001 # Time tolerance for joining KFs +unmeasured perturbation std: 0.00001 +time tolerance: 0.0025 # Time tolerance for joining KFs keyframe vote: max time span: 2.0 # seconds max buffer length: 20000 # motion deltas diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml index e2c9c2afceb37a38316c92b93bd9a419cf275a4c..678867b719b191b6ba980a5c712f5164a9f83e28 100644 --- a/src/examples/processor_imu_no_vote.yaml +++ b/src/examples/processor_imu_no_vote.yaml @@ -1,6 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails -time tolerance: 0.001 # Time tolerance for joining KFs +time tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured perturbation std: 0.00001 keyframe vote: max time span: 999999.0 # seconds max buffer length: 999999 # motion deltas diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml index 76d903cef79b11e701cf4e577942154cc56030c7..b68740d245b4922a4a095b2a0ac1b2ce5becbd52 100644 --- a/src/examples/processor_imu_t1.yaml +++ b/src/examples/processor_imu_t1.yaml @@ -1,6 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails -time tolerance: 0.001 # Time tolerance for joining KFs +unmeasured perturbation std: 0.00001 +time tolerance: 0.0025 # Time tolerance for joining KFs keyframe vote: max time span: 0.9999 # seconds max buffer length: 10000 # motion deltas diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml index 653e1e847e26d95c467170c96fdcc0cf6b145c1a..511a917c7500abbb445c7bfb016737c881dc400a 100644 --- a/src/examples/processor_imu_t6.yaml +++ b/src/examples/processor_imu_t6.yaml @@ -1,6 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails -time tolerance: 0.001 # Time tolerance for joining KFs +unmeasured perturbation std: 0.00001 +time tolerance: 0.0025 # Time tolerance for joining KFs keyframe vote: max time span: 5.9999 # seconds max buffer length: 10000 # motion deltas diff --git a/src/examples/processor_odom_3D.yaml b/src/examples/processor_odom_3D.yaml index aff35c9d2732c6489c1506021874f5325f303678..f501e333800ec1bbb4b7c751a32aa67a99bec74c 100644 --- a/src/examples/processor_odom_3D.yaml +++ b/src/examples/processor_odom_3D.yaml @@ -1,5 +1,6 @@ processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails +time tolerance: 0.01 # seconds keyframe vote: max time span: 0.2 # seconds max buffer length: 10 # motion deltas diff --git a/src/examples/processor_tracker_landmark_apriltag.yaml b/src/examples/processor_tracker_landmark_apriltag.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e8b1fd02dc80ffedefafc44ae3af9898a873cb3b --- /dev/null +++ b/src/examples/processor_tracker_landmark_apriltag.yaml @@ -0,0 +1,57 @@ +processor type: "TRACKER LANDMARK APRILTAG" +processor name: "tracker landmark apriltag example" + +detector parameters: + quad_decimate: 1.5 # doing quad detection at lower resolution to speed things up (see end of file) + quad_sigma: 0.8 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file) + nthreads: 8 # how many thread during tag detection (does not change much?) + debug: false # write some debugging images + refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff) + ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example) + ippe_max_rep_error: 2.0 # to deactivate, set at something big (100) + +tag widths: + 0: 0.055 + 1: 0.055 + 2: 0.055 + 3: 0.055 + +tag parameters: + tag_family: "tag36h11" + # tag_black_border: 1 + tag_width_default: 0.165 # used if tag width not specified + + +noise: + std_xy : 0.1 # m + std_z : 0.1 # m + std_rpy_degrees : 5 # degrees + std_pix: 20 # pixel error + +vote: + voting active: true + min_time_vote: 0 # s + max_time_vote: 0 # s + min_features_for_keyframe: 12 + max_features_diff: 17 + nb_vote_for_every_first: 50 + enough_info_necessary: true # create kf if enough information to uniquely determine the KF position (necessary for apriltag_only slam) + +reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor +add_3D_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only + + +# Annexes: +### Quad decimate: the higher, the lower the resolution +# Does nothing if <= 1.0 +# Only values taken into account: +# 1.5, 2, 3, 4 +# 1.5 -> ~*2 speed up + +# Higher values use a "bad" code according to commentaries in the library, smaller do nothing +### Gaussian blur table: +# max sigma kernel size +# 0.499 1 (disabled) +# 0.999 3 +# 1.499 5 +# 1.999 7 diff --git a/src/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp index 48831f6511f2d29689ced193e599dc4ae6292a5f..d9eec5b00f275b9ed4fc1fd2f309a73daa8a171e 100644 --- a/src/examples/solver/test_ccolamd.cpp +++ b/src/examples/solver/test_ccolamd.cpp @@ -6,7 +6,7 @@ */ // Wolf includes -#include "core/common/wolf.h" +#include "base/common/wolf.h" //std includes #include <cstdlib> diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp index 9ad57098e8e3671bcf18cd54dc458fc6d776dab4..dc4094304626dda2d5f044576da3cbaed648d377 100644 --- a/src/examples/solver/test_iQR_wolf2.cpp +++ b/src/examples/solver/test_iQR_wolf2.cpp @@ -17,9 +17,9 @@ #include <queue> //Wolf includes -#include "core/state_block/state_block.h" -#include "core/factor/factor_base.h" -#include "core/sensor/sensor_laser_2D.h" +#include "base/state_block/state_block.h" +#include "base/factor/factor_base.h" +#include "base/sensor/sensor_laser_2D.h" #include "wolf_manager.h" // wolf solver @@ -35,7 +35,7 @@ //Ceres includes #include "glog/logging.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/ceres_wrapper/ceres_manager.h" //laser_scan_utils #include "iri-algorithms/laser_scan_utils/corner_detector.h" diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp index 58e91c6d8a63e81ff0337ee1ec4a7b601cdb9de7..9174dc94f6d30e66ae586d97ecc53896e0f2522a 100644 --- a/src/examples/test_2_lasers_offline.cpp +++ b/src/examples/test_2_lasers_offline.cpp @@ -1,5 +1,5 @@ //std includes -#include "core/sensor/sensor_GPS_fix.h" +#include "base/sensor/sensor_GPS_fix.h" #include <cstdlib> #include <iostream> #include <fstream> @@ -17,12 +17,12 @@ #include "glog/logging.h" //Wolf includes -#include "core/problem/problem.h" -#include "core/processor/processor_tracker_landmark_corner.h" -#include "core/processor/processor_odom_2D.h" -#include "core/sensor/sensor_laser_2D.h" -#include "core/sensor/sensor_odom_2D.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/problem/problem.h" +#include "base/processor/processor_tracker_landmark_corner.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" // laserscanutils #include "laser_scan_utils/line_finder_iterative.h" @@ -31,7 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" -#include "core/capture/capture_pose.h" +#include "base/capture/capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1230479ab7fe8370f13c46fe40817dd61889a528 --- /dev/null +++ b/src/examples/test_apriltag.cpp @@ -0,0 +1,290 @@ +/** + * \file test_apriltag.cpp + * + * Created on: Dec 14, 2018 + * \author: Dinesh Atchtuhan + */ + +//Wolf +#include "base/wolf.h" +#include "base/rotations.h" +#include "base/problem.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_camera.h" +#include "base/processor/processor_tracker_landmark_apriltag.h" +#include "base/capture/capture_image.h" +#include "base/feature/feature_apriltag.h" + +// opencv +#include <opencv2/imgproc/imgproc.hpp> +#include "opencv2/opencv.hpp" + +// Eigen +#include <Eigen/Core> +#include <Eigen/Geometry> + +// std +#include <iostream> +#include <stdlib.h> + + +void draw_apriltag(cv::Mat image, std::vector<cv::Point2d> corners, int thickness=1, bool draw_corners=false); + + +int main(int argc, char *argv[]) +{ + /* + * HOW TO USE ? + * For now, just call the executable and append the list of images to be processed. + * The images must be placed in the root folder of your wolf project. + * Ex: + * ./test_apriltag frame1.jpg frame2.jpg frame3.jpg + */ + + using namespace wolf; + + + // General execution options + const bool APPLY_CONTRAST = false; + const bool IMAGE_OUTPUT = true; + const bool USEMAP = false; + + + WOLF_INFO( "==================== processor apriltag test ======================" ) + + std::string wolf_root = _WOLF_ROOT_DIR; + // Wolf problem + ProblemPtr problem = Problem::create("PO", 3); + ceres::Solver::Options options; + options.function_tolerance = 1e-6; + options.max_num_iterations = 100; + CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, options); + + + WOLF_INFO( "==================== Configure Problem ======================" ) + Eigen::Vector7s cam_extrinsics; cam_extrinsics << 0,0,0, 0,0,0,1; + SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml"); +// SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml"); + SensorCameraPtr sen_cam = std::static_pointer_cast<SensorCamera>(sen); + ProcessorBasePtr prc = problem->installProcessor("TRACKER LANDMARK APRILTAG", "apriltags", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml"); + + if (USEMAP){ + problem->loadMap(wolf_root + "/src/examples/maps/map_apriltag_logitech_1234.yaml"); + for (auto lmk : problem->getMap()->getLandmarkList()){ + lmk->fix(); + } + } + + // set prior + Eigen::Matrix6s covariance = Matrix6s::Identity(); + Scalar std_m; + Scalar std_deg; + if (USEMAP){ + std_m = 100; // standard deviation on original translation + std_deg = 180; // standard deviation on original rotation + } + else { + std_m = 0.00001; // standard deviation on original translation + std_deg = 0.00001; // standard deviation on original rotation + } + + covariance.topLeftCorner(3,3) = std_m*std_m * covariance.topLeftCorner(3,3); + covariance.bottomRightCorner(3,3) = (M_TORAD*std_deg)*(M_TORAD*std_deg) * covariance.bottomRightCorner(3,3); + + if (USEMAP){ + FrameBasePtr F1 = problem->setPrior((Vector7s()<<0.08, 0.15, -0.75, 0, 0, 0, 1).finished(), covariance, 0.0, 0.1); + } + else { + FrameBasePtr F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), covariance, 0.0, 0.1); + F1->fix(); + } + + // first argument is the name of the program. + // following arguments are path to image (from wolf_root) + const int inputs = argc -1; + WOLF_DEBUG("nb of images: ", inputs); + cv::Mat frame; + Scalar ts(0); + Scalar dt = 1; + + WOLF_INFO( "==================== Main loop ======================" ) + for (int input = 1; input <= inputs; input++) { + std::string path = wolf_root + "/" + argv[input]; + frame = cv::imread(path, CV_LOAD_IMAGE_COLOR); + + if( frame.data ){ //if imread succeeded + + if (APPLY_CONTRAST){ + Scalar alpha = 2.0; // to tune contrast [1-3] + int beta = 0; // to tune lightness [0-100] + // Do the operation new_image(i,j) = alpha*image(i,j) + beta + for( int y = 0; y < frame.rows; y++ ){ + for( int x = 0; x < frame.cols; x++ ){ + for( int c = 0; c < 3; c++ ){ + frame.at<cv::Vec3b>(y,x)[c] = cv::saturate_cast<uchar>( alpha*( frame.at<cv::Vec3b>(y,x)[c] ) + beta ); + } + } + } + } + + CaptureImagePtr cap = std::make_shared<CaptureImage>(ts, sen_cam, frame); + // cap->setType(argv[input]); // only for problem->print() to show img filename + cap->setName(argv[input]); + WOLF_DEBUG("Processing image...", path); + sen->process(cap); + + if (IMAGE_OUTPUT){ + cv::namedWindow( cap->getName(), cv::WINDOW_NORMAL );// Create a window for display. + } + + } + else + WOLF_WARN("could not load image ", path); + + ts += dt; + } + + + if (IMAGE_OUTPUT){ + WOLF_INFO( "==================== Draw all detections ======================" ) + for (auto F : problem->getTrajectory()->getFrameList()) + { + if (F->isKey()) + { + for (auto cap : F->getCaptureList()) + { + if (cap->getType() == "IMAGE") + { + auto img = std::static_pointer_cast<CaptureImage>(cap); + for (FeatureBasePtr f : img->getFeatureList()) + { + FeatureApriltagPtr fa = std::static_pointer_cast<FeatureApriltag>(f); + draw_apriltag(img->getImage(), fa->getTagCorners(), 1); + } + cv::imshow( img->getName(), img->getImage() ); // display original image. + cv::waitKey(1); + } + } + } + } + } + + + +// WOLF_INFO( "==================== Provide perturbed prior ======================" ) +// for (auto kf : problem->getTrajectory()->getFrameList()) +// { +// Vector7s x; +// if (kf->isKey()) +// { +// x.setRandom(); +// x.tail(4).normalize(); +// kf->setState(x); +// } +// } + + WOLF_INFO( "==================== Solve problem ======================" ) + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::FULL); // 0: nothing, 1: BriefReport, 2: FullReport + WOLF_DEBUG(report); + problem->print(3,0,1,1); + + + + WOLF_INFO("============= SOLVED PROBLEM : POS | EULER (DEG) ===============") + for (auto kf : problem->getTrajectory()->getFrameList()) + { + if (kf->isKey()) + for (auto cap : kf->getCaptureList()) + { + if (cap->getType() != "POSE") + { + Vector3s T = kf->getP()->getState(); + Vector4s qv= kf->getO()->getState(); + Vector3s e = M_TODEG * R2e(q2R(qv)); + WOLF_DEBUG("KF", kf->id(), " => ", T.transpose(), " | ", e.transpose()); + } + } + } + for (auto lmk : problem->getMap()->getLandmarkList()) + { + Vector3s T = lmk->getP()->getState(); + Vector4s qv= lmk->getO()->getState(); + Vector3s e = M_TODEG * R2e(q2R(qv)); + WOLF_DEBUG(" L", lmk->id(), " => ", T.transpose(), " | ", e.transpose()); + } + + + // =============================================== + // COVARIANCES =================================== + // =============================================== + // Print COVARIANCES of all states + WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM : POS | QUAT =======") + ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + for (auto kf : problem->getTrajectory()->getFrameList()) + if (kf->isKey()) + { + Eigen::MatrixXs cov = kf->getCovariance(); + WOLF_DEBUG("KF", kf->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt()); + } + for (auto lmk : problem->getMap()->getLandmarkList()) + { + Eigen::MatrixXs cov = lmk->getCovariance(); + WOLF_DEBUG(" L", lmk->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt()); + } + std::cout << std::endl; + + + // =============================================== + // SAVE MAP TO YAML ============================== + // =============================================== + // + // problem->saveMap(wolf_root + "/src/examples/map_apriltag_set3_HC.yaml", "set3"); + + if (IMAGE_OUTPUT){ + cv::waitKey(0); + cv::destroyAllWindows(); + } + + return 0; + +} + + +void draw_apriltag(cv::Mat image, std::vector<cv::Point2d> corners, + int thickness, bool draw_corners) { + cv::line(image, corners[0], corners[1], CV_RGB(255, 0, 0), thickness); + cv::line(image, corners[1], corners[2], CV_RGB(0, 255, 0), thickness); + cv::line(image, corners[2], corners[3], CV_RGB(0, 0, 255), thickness); + cv::line(image, corners[3], corners[0], CV_RGB(255, 0, 255), thickness); + + /////// + // Leads to implement other displays + /////// + +// const auto line_type = cv::LINE_AA; +// if (draw_corners) { +// int r = thickness; +// cv::circle(image, cv::Point2i(p[0].x, p[0].y), r, CV_RGB(255, 0, 0), -1, +// line_type); +// cv::circle(image, cv::Point2i(p[1].x, p[1].y), r, CV_RGB(0, 255, 0), -1, +// line_type); +// cv::circle(image, cv::Point2i(p[2].x, p[2].y), r, CV_RGB(0, 0, 255), -1, +// line_type); +// cv::circle(image, cv::Point2i(p[3].x, p[3].y), r, CV_RGB(255, 0, 255), -1, +// line_type); +// } + +// cv::putText(image, std::to_string(apriltag.id), +// cv::Point2f(apriltag.center.x - 5, apriltag.center.y + 5), +// cv::FONT_HERSHEY_SIMPLEX, 1, CV_RGB(255, 0, 255), 2, line_type); + + +} + +//void DrawApriltags(cv::Mat &image, const ApriltagVec &apriltags) { +// for (const auto &apriltag : apriltags) { +//// DrawApriltag(image, apriltag); +// DrawApriltag(image, apriltag, 1); +// } +//} + diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index b090a8a5a4acf010959c4e53bf006855326a5629..456367a2c67b1c98ca91e9631c3f3da47751882a 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -1,5 +1,5 @@ //std includes -#include "core/sensor/sensor_GPS_fix.h" +#include "base/sensor/sensor_GPS_fix.h" #include <cstdlib> #include <iostream> #include <fstream> @@ -17,12 +17,12 @@ #include "glog/logging.h" //Wolf includes -#include "core/problem/problem.h" -#include "core/processor/processor_tracker_landmark_corner.h" -#include "core/processor/processor_odom_2D.h" -#include "core/sensor/sensor_laser_2D.h" -#include "core/sensor/sensor_odom_2D.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/problem/problem.h" +#include "base/processor/processor_tracker_landmark_corner.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" // laserscanutils #include "laser_scan_utils/line_finder_iterative.h" @@ -31,7 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" -#include "core/capture/capture_pose.h" +#include "base/capture/capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -247,7 +247,7 @@ int main(int argc, char** argv) odom_trajectory.head(3) = ground_truth_pose; // Origin Key Frame - FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); + FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts); // Prior covariance CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp index 7e83dec251a85252b14d1014114a1089c912ba27..62ad277f98576b1bf17e17c7515d8dfb428c93be 100644 --- a/src/examples/test_ceres_2_lasers_polylines.cpp +++ b/src/examples/test_ceres_2_lasers_polylines.cpp @@ -1,5 +1,5 @@ //std includes -#include "core/sensor/sensor_GPS_fix.h" +#include "base/sensor/sensor_GPS_fix.h" #include <cstdlib> #include <iostream> #include <fstream> @@ -17,12 +17,12 @@ #include "glog/logging.h" //Wolf includes -#include "core/problem/problem.h" -#include "core/processor/processor_tracker_landmark_polyline.h" -#include "core/processor/processor_odom_2D.h" -#include "core/sensor/sensor_laser_2D.h" -#include "core/sensor/sensor_odom_2D.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/problem/problem.h" +#include "base/processor/processor_tracker_landmark_polyline.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" // laserscanutils #include "laser_scan_utils/line_finder_iterative.h" @@ -31,7 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" -#include "core/capture/capture_pose.h" +#include "base/capture/capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -254,7 +254,7 @@ int main(int argc, char** argv) odom_trajectory.head(3) = ground_truth_pose; // Origin Key Frame - FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); + FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts); // Prior covariance CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp index 21c2a8b9495dc92fa43dfae9dd0c87238971125d..561cb26764601f50108f292ae2a4bd6bb3aae645 100644 --- a/src/examples/test_diff_drive.cpp +++ b/src/examples/test_diff_drive.cpp @@ -6,11 +6,11 @@ */ //Wolf -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/sensor/sensor_diff_drive.h" -#include "core/capture/capture_wheel_joint_position.h" -#include "core/processor/processor_diff_drive.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/sensor/sensor_diff_drive.h" +#include "base/capture/capture_wheel_joint_position.h" +#include "base/processor/processor_diff_drive.h" //std #include <iostream> @@ -163,7 +163,7 @@ int main(int argc, char** argv) } // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2); const std::string sensor_name("Main Odometer"); Eigen::VectorXs extrinsics(3); diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp index 1e85039d7d5a25c6320c74d0d1914c372a9220f6..d24a715cbc8d65f2b86d67e3931b9c1199f22398 100644 --- a/src/examples/test_eigen_quaternion.cpp +++ b/src/examples/test_eigen_quaternion.cpp @@ -6,7 +6,7 @@ #include <eigen3/Eigen/Geometry> //Wolf -#include "core/common/wolf.h" +#include "base/common/wolf.h" int main() { diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp index 03d7791ab0d29531fdfd8b90d2b6fb638de3ad0c..bd1ad546de08195e53d2608e2032ac1229f6142f 100644 --- a/src/examples/test_factor_AHP.cpp +++ b/src/examples/test_factor_AHP.cpp @@ -1,10 +1,10 @@ -#include "core/pinhole_tools.h" -#include "core/landmark/landmark_AHP.h" -#include "core/factor/factor_AHP.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" -#include "core/sensor/sensor_camera.h" -#include "core/capture/capture_image.h" +#include "base/math/pinhole_tools.h" +#include "base/landmark/landmark_AHP.h" +#include "base/factor/factor_AHP.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" +#include "base/sensor/sensor_camera.h" +#include "base/capture/capture_image.h" int main() { @@ -21,7 +21,7 @@ int main() //===================================================== // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 3D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3); /* Do this while there aren't extrinsic parameters on the yaml */ Eigen::Vector7s extrinsic_cam; diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp index 9d7d73379708fe897e8ec0b7bb00e39fda7f8559..4040194b382e9db0698b2a5d8acf0f852af1286b 100644 --- a/src/examples/test_factor_imu.cpp +++ b/src/examples/test_factor_imu.cpp @@ -1,14 +1,14 @@ //Wolf -#include "core/capture/capture_IMU.h" -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/capture/capture_pose.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/factor/factor_odom_3D.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/capture/capture_pose.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/factor/factor_odom_3D.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" +#include "base/ceres_wrapper/ceres_manager.h" //#define DEBUG_RESULTS @@ -80,7 +80,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature @@ -141,7 +141,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature @@ -199,7 +199,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature diff --git a/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp index 4414520dc1fc0edf2260ad4cf416ee3ddd2ea56d..e095e561d3348cb3ff4445c5f874e8f34ca637e9 100644 --- a/src/examples/test_faramotics_simulation.cpp +++ b/src/examples/test_faramotics_simulation.cpp @@ -16,10 +16,10 @@ #include "unistd.h" // wolf -#include "core/common/wolf.h" -#include "core/feature/feature_base.h" -#include "core/landmark/landmark_base.h" -#include "core/state_block/state_block.h" +#include "base/common/wolf.h" +#include "base/feature/feature_base.h" +#include "base/landmark/landmark_base.h" +#include "base/state_block/state_block.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp index 842a6a14b6e682da3481a7f3bd51d80e819fb857..3997b9476334d39092ee2eaf5140c5b9a9facfc0 100644 --- a/src/examples/test_image.cpp +++ b/src/examples/test_image.cpp @@ -52,7 +52,7 @@ int main(int argc, char** argv) std::string wolf_root = _WOLF_ROOT_DIR; std::cout << "Wolf root: " << wolf_root << std::endl; - ProblemPtr wolf_problem_ = Problem::create("PO 3D"); + ProblemPtr wolf_problem_ = Problem::create("PO", 3); //===================================================== // Method 1: Use data generated here for sensor and processor diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp index c05fbbf9c4e676c5611c7073b94b9f55385a9267..dab6894d7a558cf53502ffc58c6a407a3fa4bbe5 100644 --- a/src/examples/test_imuDock.cpp +++ b/src/examples/test_imuDock.cpp @@ -5,21 +5,21 @@ * \author: Dinesh Atchuthan */ -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/sensor/sensor_odom_3D.h" -#include "core/processor/processor_odom_3D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" //Factors headers -#include "core/factor/factor_fix_bias.h" +#include "base/factor/factor_fix_bias.h" //std #include <iostream> #include <fstream> -#include "core/factor/factor_pose_3D.h" +#include "base/factor/factor_pose_3D.h" #define OUTPUT_RESULTS //#define ADD_KF3 diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp index 039615445807ab6ef9e1dadab7e273135bc650ef..43a54d654c1611e7d9c5a42ef326c382f2dd9566 100644 --- a/src/examples/test_imuDock_autoKFs.cpp +++ b/src/examples/test_imuDock_autoKFs.cpp @@ -5,21 +5,21 @@ * \author: Dinesh Atchuthan */ -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/sensor/sensor_odom_3D.h" -#include "core/processor/processor_odom_3D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" //Factors headers -#include "core/factor/factor_fix_bias.h" +#include "base/factor/factor_fix_bias.h" //std #include <iostream> #include <fstream> -#include "core/factor/factor_pose_3D.h" +#include "base/factor/factor_pose_3D.h" #define OUTPUT_RESULTS //#define AUTO_KFS diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp index e0e4e4778e91090221d70121bd89907ad3756823..4b923de89990eac72ffc20df61b13e28dbb76b66 100644 --- a/src/examples/test_imuPlateform_Offline.cpp +++ b/src/examples/test_imuPlateform_Offline.cpp @@ -1,15 +1,15 @@ //Wolf -#include "core/capture/capture_IMU.h" -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/sensor/sensor_odom_3D.h" -#include "core/factor/factor_odom_3D.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" -#include "core/processor/processor_odom_3D.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/factor/factor_odom_3D.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" +#include "base/processor/processor_odom_3D.h" +#include "base/ceres_wrapper/ceres_manager.h" //std #include <iostream> diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp index 817b08a14a4c71caf9f9e4807c71cc14cc43d78e..e542e18e0ffac35b9c398ba01ab23bd7fc2e13a1 100644 --- a/src/examples/test_imu_constrained0.cpp +++ b/src/examples/test_imu_constrained0.cpp @@ -1,15 +1,15 @@ //Wolf -#include "core/capture/capture_IMU.h" -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/sensor/sensor_odom_3D.h" -#include "core/factor/factor_odom_3D.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" -#include "core/processor/processor_odom_3D.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/factor/factor_odom_3D.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" +#include "base/processor/processor_odom_3D.h" +#include "base/ceres_wrapper/ceres_manager.h" //std #include <iostream> diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp index ea1079d75c7f11cb3de61fe55df12ce3bd39c441..9e01558e3bc013c65d553d7e213785fdacfaaf96 100644 --- a/src/examples/test_kf_callback.cpp +++ b/src/examples/test_kf_callback.cpp @@ -16,7 +16,7 @@ int main() using namespace Eigen; using namespace std; - ProblemPtr problem = Problem::create("PO 2D"); + ProblemPtr problem = Problem::create("PO", 2); SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),""); ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>(); diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp index 070300d90efc27cedd05e39cb3d7f100a7ad3db9..159ce8532349020a8f04107719f5f810db6da651 100644 --- a/src/examples/test_map_yaml.cpp +++ b/src/examples/test_map_yaml.cpp @@ -5,13 +5,13 @@ * \author: jsola */ -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/map_base.h" -#include "core/landmark/landmark_polyline_2D.h" -#include "core/landmark/landmark_AHP.h" -#include "core/state_block/state_block.h" -#include "core/yaml/yaml_conversion.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/map/map_base.h" +#include "base/landmark/landmark_polyline_2D.h" +#include "base/landmark/landmark_AHP.h" +#include "base/state_block/state_block.h" +#include "base/yaml/yaml_conversion.h" #include <iostream> using namespace wolf; @@ -75,7 +75,7 @@ int main() std::string wolf_config = wolf_root + "/src/examples"; std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl; - ProblemPtr problem = Problem::create("PO 2D"); + ProblemPtr problem = Problem::create("PO", 2); filename = wolf_config + "/map_polyline_example.yaml"; std::cout << "Reading map from file: " << filename << std::endl; problem->loadMap(filename); diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp index f2d5bbade62637c592b5f2dd0cf2341d825dded8..1fbed214fb99a53c9c4d509ccc3e47d27c744cb1 100644 --- a/src/examples/test_mpu.cpp +++ b/src/examples/test_mpu.cpp @@ -6,11 +6,11 @@ */ //Wolf -#include "core/capture/capture_IMU.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" +#include "base/capture/capture_IMU.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> @@ -18,8 +18,8 @@ #include <cmath> #include <termios.h> #include <fcntl.h> -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" #define DEBUG_RESULTS #define FROM_FILE diff --git a/src/examples/test_polylines.cpp b/src/examples/test_polylines.cpp index b972424c7c86018386d2ee0b5fded686b7239c75..39943f93271bf42f65f79d016966cbde9bbdbc01 100644 --- a/src/examples/test_polylines.cpp +++ b/src/examples/test_polylines.cpp @@ -206,7 +206,7 @@ int main(int argc, char** argv) // ------------------------ START EXPERIMENT ------------------------ // First frame FIXED - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0)); + last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0)); last_frame_ptr->fix(); bl_problem_ptr->print(4, true, false, true); @@ -228,7 +228,7 @@ int main(int argc, char** argv) Eigen::Vector3s from_pose = frame_from_ptr->getState(); R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix(); Eigen::Vector3s new_frame_pose = from_pose + R*meas; - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to))); + last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to))); frame_to_ptr = last_frame_ptr; diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp index ec77aedeee3242daffe10d89c1444fa9ea1bf1f9..33ec4cabb2580353719cf4d50ed14976e626cfaa 100644 --- a/src/examples/test_processor_imu.cpp +++ b/src/examples/test_processor_imu.cpp @@ -6,13 +6,13 @@ */ //Wolf -#include "core/capture/capture_IMU.h" -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp index 988244d1598d3a8a9a1bdebf0144266b6744044b..22c797d6d0726e600427680bd475dede67ebc5b8 100644 --- a/src/examples/test_processor_imu_jacobians.cpp +++ b/src/examples/test_processor_imu_jacobians.cpp @@ -6,13 +6,13 @@ */ //Wolf -#include "core/capture/capture_IMU.h" -#include "core/sensor/sensor_IMU.h" +#include "base/capture/capture_IMU.h" +#include "base/sensor/sensor_IMU.h" #include <test/processor_IMU_UnitTester.h> -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/state_block/state_block.h" -#include "core/state_block/state_quaternion.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/state_block/state_block.h" +#include "base/state_block/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp index 1be3716509252cb39713d8cbc97209675f438a2c..2427dc24b0b55da36640f9175bcdc7a6eea0b4bc 100644 --- a/src/examples/test_processor_odom_3D.cpp +++ b/src/examples/test_processor_odom_3D.cpp @@ -5,13 +5,13 @@ * \author: jsola */ -#include "core/capture/capture_IMU.h" -#include "core/problem/problem.h" -#include "core/sensor/sensor_odom_2D.h" -#include "core/processor/processor_odom_3D.h" -#include "core/map_base.h" -#include "core/landmark/landmark_base.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/problem/problem.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_3D.h" +#include "base/map/map_base.h" +#include "base/landmark/landmark_base.h" +#include "base/ceres_wrapper/ceres_manager.h" #include <cstdlib> @@ -40,7 +40,7 @@ int main (int argc, char** argv) } cout << "Final timestamp tf = " << tf.get() << " s" << endl; - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); ceres::Solver::Options ceres_options; // ceres_options.max_num_iterations = 1000; // ceres_options.function_tolerance = 1e-10; diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp index a5c2476d700e0651bc027787e2b58d6b9e339e78..eccd7b49e4d920ed03db8fff1a2963fbac5b93fa 100644 --- a/src/examples/test_processor_tracker_feature.cpp +++ b/src/examples/test_processor_tracker_feature.cpp @@ -9,37 +9,12 @@ #include <iostream> //Wolf -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/sensor/sensor_base.h" -#include "core/state_block/state_block.h" -#include "core/processor/processor_tracker_feature_dummy.h" -#include "core/capture/capture_void.h" - -void print_prc(wolf::ProcessorTrackerFeaturePtr _prc) -{ - using namespace wolf; - - auto o = _prc->getOrigin(); - auto l = _prc->getLast(); - auto i = _prc->getIncoming(); - - std::cout << "o: C" << ( int)( o ? o->id() : -99 ) - << " || l: C" << ( int)( l ? l->id() : -99 ) - << " || i: C" << ( int)( i ? i->id() : -99 ) - << std::endl; - for (auto ftr : _prc->getLast()->getFeatureList()) - { - SizeStd trk_id = ftr->trackId(); - std::cout << "Track " << trk_id << " ---------------------" << std::endl; - for (auto ftr_pair : _prc->track(trk_id)) - { - auto f = ftr_pair.second; - auto C = f->getCapture(); - std::cout << "f" << f->id() << " C" << (int)(C ? C->id() : -99) << std::endl; - } - } -} +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/state_block/state_block.h" +#include "base/processor/processor_tracker_feature_dummy.h" +#include "base/capture/capture_void.h" int main() { @@ -51,15 +26,14 @@ int main() std::cout << std::endl << "==================== processor tracker feature test ======================" << std::endl; // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2); SensorBasePtr sensor_ptr_ = make_shared<SensorBase>("ODOM 2D", 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); ProcessorParamsTrackerFeaturePtr params_trk = std::make_shared<ProcessorParamsTrackerFeature>(); - params_trk->voting_active = true; - params_trk->max_new_features = 7; - params_trk->min_features_for_keyframe = 4; + 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); @@ -72,11 +46,7 @@ int main() Scalar dt = 0.5; for (auto i = 0; i < 10; i++) { - std::cout << "\n===== Capture TS = " << t << " =====" << std::endl; processor_ptr_->process(make_shared<CaptureVoid>(t, sensor_ptr_)); - - print_prc(processor_ptr_); - t += dt; } diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp index 05eb1a5194b43dadb453120af570ebbc342fb7e3..b3ce8be59ead808976a1909982ea307e2ab675ef 100644 --- a/src/examples/test_processor_tracker_landmark.cpp +++ b/src/examples/test_processor_tracker_landmark.cpp @@ -9,12 +9,12 @@ #include <iostream> //Wolf -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/sensor/sensor_base.h" -#include "core/state_block/state_block.h" -#include "core/processor/processor_tracker_landmark_dummy.h" -#include "core/capture/capture_void.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/state_block/state_block.h" +#include "base/processor/processor_tracker_landmark_dummy.h" +#include "base/capture/capture_void.h" void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_) { @@ -61,19 +61,22 @@ int main() std::cout << std::endl << "==================== processor tracker landmark test ======================" << std::endl; // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); - SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", 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); - + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2); + // SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", 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); + auto sensor_ptr_ = SensorBase::emplace<SensorBase>(wolf_problem_ptr_->getHardware(), "ODOM 2D", 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); 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_); + // std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk); + std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = + std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(ProcessorBase::emplace<ProcessorTrackerLandmarkDummy>(sensor_ptr_, params_trk)); + // wolf_problem_ptr_->addSensor(sensor_ptr_); + // sensor_ptr_->addProcessor(processor_ptr_); std::cout << "sensor & processor created and added to wolf problem" << std::endl; diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp index f3c79633d5b27a3ec3fc84bb52124eb080380993..8ac67cf394349831b4acfb2ed30d8b84cf7457a8 100644 --- a/src/examples/test_processor_tracker_landmark_image.cpp +++ b/src/examples/test_processor_tracker_landmark_image.cpp @@ -1,18 +1,18 @@ //std #include <iostream> -#include "core/processor/processor_tracker_landmark_image.h" +#include "base/processor/processor_tracker_landmark_image.h" //Wolf -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/state_block/state_block.h" -#include "core/processor/processor_odom_3D.h" -#include "core/sensor/sensor_odom_3D.h" -#include "core/sensor/sensor_camera.h" -#include "core/capture/capture_image.h" -#include "core/capture/capture_pose.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/state_block/state_block.h" +#include "base/processor/processor_odom_3D.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/sensor/sensor_camera.h" +#include "base/capture/capture_image.h" +#include "base/capture/capture_pose.h" +#include "base/ceres_wrapper/ceres_manager.h" // Vision utils includes #include <vision_utils.h> @@ -80,7 +80,7 @@ int main(int argc, char** argv) //===================================================== // Wolf problem - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); // ODOM SENSOR AND PROCESSOR SensorBasePtr sensor_base = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml"); @@ -98,7 +98,7 @@ int main(int argc, char** argv) //===================================================== // Origin Key Frame is fixed TimeStamp t = 0; - FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t); + FrameBasePtr origin_frame = problem->emplaceFrame(KEY, (Vector7s()<<1,0,0,0,0,0,1).finished(), t); problem->getProcessorMotion()->setOrigin(origin_frame); origin_frame->fix(); diff --git a/src/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp index e643489cace9354674274e09a23858614e259fca..b9f01912d4341df33d3fd270ef8d66c9e15f4477 100644 --- a/src/examples/test_projection_points.cpp +++ b/src/examples/test_projection_points.cpp @@ -6,7 +6,7 @@ #include <iostream> //wolf includes -#include "core/pinhole_tools.h" +#include "base/math/pinhole_tools.h" int main(int argc, char** argv) { diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp index b901a0a10514bda807dbade90b14f6f352997ccc..23ea39de70d2ed83db091bab3e32d3520c4cd844 100644 --- a/src/examples/test_simple_AHP.cpp +++ b/src/examples/test_simple_AHP.cpp @@ -5,15 +5,15 @@ * \author: jtarraso */ -#include "core/common/wolf.h" -#include "core/frame/frame_base.h" -#include "core/pinhole_tools.h" -#include "core/sensor/sensor_camera.h" -#include "core/rotations.h" -#include "core/capture/capture_image.h" -#include "core/landmark/landmark_AHP.h" -#include "core/factor/factor_AHP.h" -#include "core/ceres_wrapper/ceres_manager.h" +#include "base/common/wolf.h" +#include "base/frame/frame_base.h" +#include "base/math/pinhole_tools.h" +#include "base/sensor/sensor_camera.h" +#include "base/math/rotations.h" +#include "base/capture/capture_image.h" +#include "base/landmark/landmark_AHP.h" +#include "base/factor/factor_AHP.h" +#include "base/ceres_wrapper/ceres_manager.h" // Vision utils #include <vision_utils/vision_utils.h> @@ -95,13 +95,13 @@ int main(int argc, char** argv) // ============================================================================================================ /* 1 */ - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); // One anchor frame to define the lmk, and a copy to make a factor - FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); // and two other frames to observe the lmk - FrameBasePtr kf_3 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_4 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0)); kf_1->fix(); kf_2->fix(); diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp index 4edc04ea99be9fd5357a57e6e32f4ab6de0bdb4f..036ed1250c8dea7c1aadf10e0c437eda243e5ecf 100644 --- a/src/examples/test_sort_keyframes.cpp +++ b/src/examples/test_sort_keyframes.cpp @@ -6,10 +6,10 @@ */ // Wolf includes -#include "core/common/wolf.h" -#include "core/problem/problem.h" -#include "core/frame/frame_base.h" -#include "core/trajectory_base.h" +#include "base/common/wolf.h" +#include "base/problem/problem.h" +#include "base/frame/frame_base.h" +#include "base/trajectory/trajectory_base.h" // STL includes #include <list> @@ -31,22 +31,22 @@ int main() { ProblemPtr problem_ptr = Problem::create(FRM_PO_2D); - problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1)); - FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2)); - FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3)); - problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4)); - FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5)); - FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6)); + problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1)); + FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2)); + FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3)); + problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4)); + FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5)); + FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6)); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl; - frm5->setKey(); + frm5->setEstimated(); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl; - frm2->setKey(); + frm2->setEstimated(); printFrames(problem_ptr); @@ -56,21 +56,21 @@ int main() printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl; - frm3->setKey(); + frm3->setEstimated(); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl; - frm6->setKey(); + frm6->setEstimated(); printFrames(problem_ptr); - FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7)); + FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7)); std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl; printFrames(problem_ptr); - FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35)); + FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35)); std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl; printFrames(problem_ptr); diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp index fa577c15b88f05a9f9f94a7f63eb926f493cf2c7..a521cabd1b5b1276846573df70b53d2b70b6d537 100644 --- a/src/examples/test_sparsification.cpp +++ b/src/examples/test_sparsification.cpp @@ -216,7 +216,7 @@ int main(int argc, char** argv) // ------------------------ START EXPERIMENT ------------------------ // First frame FIXED - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0)); + last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0)); last_frame_ptr->fix(); bl_problem_ptr->print(4, true, false, true); @@ -238,7 +238,7 @@ int main(int argc, char** argv) Eigen::Vector3s from_pose = frame_from_ptr->getState(); R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix(); Eigen::Vector3s new_frame_pose = from_pose + R*meas; - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to))); + last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to))); frame_to_ptr = last_frame_ptr; diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp index 51cf76ffc329a9a3bbc62133ce24d304c26fbc7f..0fabac44e3252fafad9e940e9bcfd1b62b0e8ec0 100644 --- a/src/examples/test_state_quaternion.cpp +++ b/src/examples/test_state_quaternion.cpp @@ -5,9 +5,9 @@ * \author: jsola */ -#include "core/frame/frame_base.h" -#include "core/state_block/state_quaternion.h" -#include "core/time_stamp.h" +#include "base/frame/frame_base.h" +#include "base/state_block/state_quaternion.h" +#include "base/common/time_stamp.h" #include <iostream> diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp index e4adbd77f32ba90d87443986a62d9ffec24ffa08..b608bb9583320b361d88ea2a8c48ff973e85c08d 100644 --- a/src/examples/test_wolf_factories.cpp +++ b/src/examples/test_wolf_factories.cpp @@ -5,21 +5,21 @@ * \author: jsola */ -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_GPS_fix.h" -#include "core/hardware_base.h" -#include "core/sensor/sensor_camera.h" -#include "core/sensor/sensor_odom_2D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_GPS_fix.h" +#include "base/hardware/hardware_base.h" +#include "base/sensor/sensor_camera.h" +#include "base/sensor/sensor_odom_2D.h" #include "../sensor_imu.h" //#include "../sensor_gps.h" -#include "core/processor/processor_odom_2D.h" -#include "core/processor/processor_odom_3D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/processor/processor_odom_3D.h" #include "../processor_image_feature.h" -#include "core/problem/problem.h" +#include "base/problem/problem.h" -#include "core/factory.h" +#include "base/common/factory.h" #include <iostream> #include <iomanip> diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp index 9463399ff7843eca655e702d768a7b62fe383528..b87d3c17ec366f3d868aafb4bf4397b17ba92f45 100644 --- a/src/examples/test_wolf_imported_graph.cpp +++ b/src/examples/test_wolf_imported_graph.cpp @@ -130,8 +130,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store diff --git a/src/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp index a4f3eb8a48f33df6234f9767170ca480bc7bbe55..3b7bdfab70281f10e27b17891564a9b633ac92e1 100644 --- a/src/examples/test_wolf_logging.cpp +++ b/src/examples/test_wolf_logging.cpp @@ -5,8 +5,8 @@ * \author: Jeremie Deray */ -#include "core/common/wolf.h" -#include "core/logging.h" +#include "base/common/wolf.h" +#include "base/utils/logging.h" int main(int, char*[]) { diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp index de917e471e6535c626561ab398e8bf00d3637725..99567b5c3fb991e7664b255fc3893df9f027e1c8 100644 --- a/src/examples/test_wolf_prunning.cpp +++ b/src/examples/test_wolf_prunning.cpp @@ -148,8 +148,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store diff --git a/src/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp index 4ea048471753a28281c01dc50f3fcda0316f0bd7..ff78c97b0866ab66440ba91d20d8d0db503aed26 100644 --- a/src/examples/test_wolf_root.cpp +++ b/src/examples/test_wolf_root.cpp @@ -6,7 +6,7 @@ */ //Wolf -#include "core/common/wolf.h" +#include "base/common/wolf.h" //std #include <iostream> diff --git a/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp index f67833444f5f6818100a0a960f34369b9d0e8338..5181d73acd049da2469ae0cf6a61296317b865df 100644 --- a/src/examples/test_yaml.cpp +++ b/src/examples/test_yaml.cpp @@ -5,10 +5,10 @@ * \author: jsola */ -#include "core/pinhole_tools.h" +#include "base/math/pinhole_tools.h" #include "yaml/yaml_conversion.h" #include "processor_image_feature.h" -#include "core/factory.h" +#include "base/common/factory.h" #include <yaml-cpp/yaml.h> diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index dcf08db234a0a4f52a600245b85a6e487b917b35..9cf010e0eaf62fe4e74d4f0c241e088bc3e798aa 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/src/processor/processor_gnss_fix.cpp @@ -46,7 +46,7 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr) // MAKE KF else if (voteForKeyFrame() && permittedKeyFrame()) { - new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _capture_ptr->getTimeStamp()); + new_frame_ptr = getProblem()->emplaceFrame(KEY, _capture_ptr->getTimeStamp()); getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance); //WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp()); } diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp index f9b62fd70dbd9f98d7aceb1adc1b920c240a5ea8..d72c8b97adc09e9686e7eca4a18635033031dd33 100644 --- a/src/processor/processor_gnss_single_diff.cpp +++ b/src/processor/processor_gnss_single_diff.cpp @@ -38,7 +38,7 @@ void ProcessorGnssSingleDiff::process(CaptureBasePtr _capture_ptr) // MAKE KF else if (voteForKeyFrame() && permittedKeyFrame()) { - new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _capture_ptr->getTimeStamp()); + new_frame_ptr = getProblem()->emplaceFrame(KEY, _capture_ptr->getTimeStamp()); getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance); WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp()); } diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp index 555fb301f7d48618bb783fad0b6b386057fbda39..988ed119d7af519118f9fac6fc306c4ea39eee05 100644 --- a/test/gtest_factor_gnss_fix_2D.cpp +++ b/test/gtest_factor_gnss_fix_2D.cpp @@ -88,7 +88,7 @@ Matrix3s R_map_base = AngleAxis<Scalar>(o_map_base(0), Vector3s::UnitZ()).toRota Vector3s t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu; // WOLF -ProblemPtr problem_ptr = Problem::create("PO 2D"); +ProblemPtr problem_ptr = Problem::create("PO", 2); CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsGnss>())); FrameBasePtr frame_ptr; @@ -113,7 +113,7 @@ TEST(FactorGnssFix2DTest, configure_tree) // Emplace a frame (FIXED) Vector3s frame_pose = (Vector3s() << t_map_base(0), t_map_base(1), o_map_base(0)).finished(); - frame_ptr = problem_ptr->emplaceFrame(KEY_FRAME, frame_pose, TimeStamp(0)); + frame_ptr = problem_ptr->emplaceFrame(KEY, frame_pose, TimeStamp(0)); problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0); // Create & process GNSS Fix capture diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp index ca243a4ebb31650005f84c030c8f7ee0d325c34b..cabc1f8cb9a3c76a5859b4ff6a31a977fb157cd7 100644 --- a/test/gtest_factor_gnss_single_diff_2D.cpp +++ b/test/gtest_factor_gnss_single_diff_2D.cpp @@ -59,7 +59,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test // ---------------------------------------------------- // Problem and solver - problem_ptr = Problem::create("PO 2D"); + problem_ptr = Problem::create("PO", 2); ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); ceres_mgr_ptr->getSolverOptions().max_num_iterations = 10; diff --git a/wolf_scripts/create_plugin.sh b/wolf_scripts/create_plugin.sh new file mode 100755 index 0000000000000000000000000000000000000000..b1ca42f945f7f559a70e43377412faa8e6d4ec9c --- /dev/null +++ b/wolf_scripts/create_plugin.sh @@ -0,0 +1,126 @@ +#!/bin/bash +# $1 path to the root of the plugin +# $2 name of the plugin +# $3 files to be moved + +#Generate the necessary dirs +# if [ ! -d $1 ]; +# then +# mkdir $1 +# fi + +# if [ ! -d $1/include/$2 ]; +# then +# # mkdir $1/include +# mkdir $1/include/$2 +# fi +# if [ ! -d $1/src ]; +# then +# mkdir $1/src +# fi +root_dir=$(echo $1 | rev | cut -d '/' -f 2- | rev) +if [ ! -d $root_dir/$2 ]; +then + cp -a ../Skeleton $root_dir + mv $root_dir/Skeleton $root_dir/$2 + mv $root_dir/$2/include/skeleton $root_dir/$2/include/$2 +fi + +for folder in capture factor feature landmark processor sensor yaml ; do + if [ ! -d $1/include/$2/$folder ]; + then + mkdir $1/include/$2/$folder + fi + if [ ! -d $1/src/$folder ]; + then + mkdir $1/src/$folder + fi +done +for file in $(cat $3); do + head=$(echo $file | cut -d '/' -f 1) + if [ "$head" = "include" ]; + then + folder=$(echo $file | cut -d '/' -f 3) + suffix=$(echo $file | cut -d '/' -f 4-) + line=$(ag "HDRS_"${folder^^} $1/CMakeLists.txt | cut -d ':' -f 1 | head -1) + line=$(($line + 1)) + echo $line " && " $file " && " $folder " && " $suffix + if [ "$suffix" = "" ]; + then + line=$(ag "HDRS_BASE" $1/CMakeLists.txt | cut -d ':' -f 1 | head -1) + line=$(($line + 1)) + suffix=$folder + sed -i ""$line"i $head/$2/$suffix" $1/CMakeLists.txt + cp $file $1/$head/$2/$suffix + else + sed -i ""$line"i $head/$2/$folder/$suffix" $1/CMakeLists.txt + cp $file $1/$head/$2/$folder/$suffix + fi + elif [ "$head" = "src" ]; + then + folder=$(echo $file | cut -d '/' -f 2) + suffix=$(echo $file | cut -d '/' -f 3-) + # ag "SRCS_"$folder $1/CMakeLists.txt + line=$(ag "SRCS_"${folder^^} $1/CMakeLists.txt | cut -d ':' -f 1 | head -1) + line=$(($line + 1)) + echo $line " && " $file " && " $folder " && " $suffix + if [ "$suffix" = "" ]; + then + line=$(ag "SRCS_BASE" $1/CMakeLists.txt | cut -d ':' -f 1 | head -1) + line=$(($line + 1)) + suffix=$folder + sed -i ""$line"i $file" $1/CMakeLists.txt + cp $file $1/$head/$suffix + else + sed -i ""$line"i $file" $1/CMakeLists.txt + cp $file $1/$head/$folder/$suffix + fi + else + cp $file $1/$file + fi +done +for f in $(cat $3); do + hhead=$(echo $f | cut -d '/' -f 1) + if [ "$hhead" = "include" ]; + then + ffolder=$(echo $f | cut -d '/' -f 3) + ssuffix=$(echo $f | cut -d '/' -f 4-) + inc=$ffolder/$ssuffix + else + continue + fi + for ff in $(cat $3); do + head=$(echo $ff | cut -d '/' -f 1) + if [ "$head" = "include" ]; + then + folder=$(echo $ff | cut -d '/' -f 3) + suffix=$(echo $ff | cut -d '/' -f 4-) + if [ "$suffix" = "" ]; + then + new_path=$1/$head/$2/$folder + sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path + else + new_path=$1/$head/$2/$folder/$suffix + sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path + fi + # sed -n -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@pg" $new_path + # sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path + elif [ "$head" = "src" ]; + then + folder=$(echo $ff | cut -d '/' -f 2) + suffix=$(echo $ff | cut -d '/' -f 3-) + # sed -n -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@pg" $new_path + if [ "$suffix" = "" ]; + then + new_path=$1/$head/$folder + sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path + else + new_path=$1/$head/$folder/$suffix + sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $new_path + fi + else + # sed -n -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@pg" $new_path + sed -i -E "s@(#include[[:space:]]+\")base(\/$inc\")@\1$2\2@g" $1/$ff + fi + done +done \ No newline at end of file diff --git a/wolf_scripts/include_refactor.sh b/wolf_scripts/include_refactor.sh index 6d2c2fd8d6fae80bb3c327ead22dfa4c097de20b..2801608bc3747dc71915205f13833c4a6ab36d23 100755 --- a/wolf_scripts/include_refactor.sh +++ b/wolf_scripts/include_refactor.sh @@ -1,17 +1,36 @@ #!/bin/bash -for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do - for f in $(cat ~/workspace/wip/wolf/files.txt); do - path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-) - matches=$(echo $path | wc -w) - if [ $matches -gt 1 ]; then - # echo $f " -> " $path - path=$(echo $path | cut -d ' ' -f 1) - fi - # echo $f " now in -> " $path " modifying file "$ff - # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff - sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff +for folder in problem hardware trajectory map frame state_block common math utils; do + for ff in $(find include/base/$folder src/$folder -type f); do + name=$(echo $ff | rev | cut -d '/' -f 1 | rev) + old="base/$name" + new="base/$folder/$name" + # echo "%%%%%%%%% "$ff " ¬¬ $name" + # echo "$old ºº $new" + # for target in $(find include/base src test -type f); do + for target in $(find hello_wolf -type f); do + # out=$(sed -E -n "s:$old:$new:gp" $target) + out=$(sed -i -E "s:$old:$new:g" $target) + if [[ $out ]]; then + echo ">>> changing : $old -> $new @ $target" + echo $out + fi + done done done + +# for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do +# for f in $(cat ~/workspace/wip/wolf/files.txt); do +# path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-) +# matches=$(echo $path | wc -w) +# if [ $matches -gt 1 ]; then +# # echo $f " -> " $path +# path=$(echo $path | cut -d ' ' -f 1) +# fi +# # echo $f " now in -> " $path " modifying file "$ff +# # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff +# sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff +# done +# done # for f in $(cat ~/workspace/wip/wolf/files.txt); do # path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 7-) # matches=$(echo $path | wc -w) diff --git a/wolf_scripts/out b/wolf_scripts/out index 93fc67671cc2375e1857e43c0b57ff52ad2b7b35..2b7857b6ba924b96ce256a0a7661b7079c429a2a 100644 --- a/wolf_scripts/out +++ b/wolf_scripts/out @@ -132,7 +132,7 @@ -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ================================== -=============================================================== -+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/new_processor_factory.h ================================== ++====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_processor_factory.h ================================== -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ================================== -=============================================================== @@ -146,7 +146,7 @@ -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ================================== -=============================================================== -+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/new_sensor_factory.h ================================== ++====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_sensor_factory.h ================================== -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ================================== -=============================================================== @@ -273,8 +273,8 @@ #include "factory.h" #include "sensor_factory.h" #include "processor_factory.h" -#include "new_sensor_factory.h" -#include "new_processor_factory.h" +#include "autoconf_sensor_factory.h" +#include "autoconf_processor_factory.h" #include "constraint_base.h" #include "state_block.h" #include "processor_motion.h"