Commit dbd639db authored by mederic_fourmy's avatar mederic_fourmy
Browse files

Merge branch 'devel' into 19-apriltag-factor-projection

parents dbe37283 81354c3e
......@@ -4,7 +4,10 @@
README.txt
bin/
build/
build_debug/
build_release/
build-debug/
build-release/
lib/
.idea/
./Wolf.user
......@@ -31,9 +34,6 @@ src/examples/map_apriltag_save.yaml
demos/map_apriltag_save.yaml
\.vscode/
build_release/
apriltag.found
apriltagw.found
Testing/Temporary/LastTest.log
Testing/Temporary/CTestCostData.txt
......@@ -3,6 +3,12 @@ stages:
- build_and_test
############ YAML ANCHORS ############
.print_variables_template: &print_variables_definition
# Print variables
- echo $CI_COMMIT_BRANCH
- echo $WOLF_VISION_BRANCH
- echo $WOLF_APRILTAG_BRANCH
.preliminaries_template: &preliminaries_definition
## Install ssh-agent if not already installed, it is required by Docker.
## (change apt-get to yum if you use an RPM-based image)
......@@ -154,9 +160,8 @@ license_headers:
- key: wolf-focal
paths:
- ci_deps/wolf/
except:
- master
before_script:
- *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
script:
......@@ -179,9 +184,8 @@ build_and_test:bionic:
- key: apriltaglib-bionic
paths:
- ci_deps/apriltaglib/
except:
- master
before_script:
- *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_visionutils_definition
......@@ -208,9 +212,8 @@ build_and_test:focal:
- key: apriltaglib-focal
paths:
- ci_deps/apriltaglib/
except:
- master
before_script:
- *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_visionutils_definition
......
# Pre-requisites about cmake itself
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH true)
......@@ -16,7 +12,8 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
set(INCLUDE_INSTALL_DIR include/wolf)
set(LIB_INSTALL_DIR lib/)
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
......@@ -67,10 +64,6 @@ if(BUILD_TESTS)
enable_testing()
endif()
#CMAKE modules
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
MESSAGE(STATUS ${CMAKE_MODULE_PATH})
# Some wolf compilation options
IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
set(_WOLF_DEBUG true)
......@@ -80,12 +73,14 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============
FIND_PACKAGE(wolfcore REQUIRED CONFIG)
FIND_PACKAGE(wolfvision REQUIRED)
FIND_PACKAGE(apriltag REQUIRED)
#============ CONFIG.H ============
string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
# variable used to compile the config.h.in file
string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
......@@ -105,26 +100,23 @@ message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
# ============ INCLUDES ============
INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${wolfvision_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
INCLUDE_DIRECTORIES(${APRILTAG_INCLUDE_DIRS})
# ============ HEADERS ============
SET(HDRS_FACTOR
include/apriltag/factor/factor_apriltag.h
include/apriltag/factor/factor_apriltag_proj.h
include/${PROJECT_NAME}/factor/factor_apriltag.h
include/${PROJECT_NAME}/factor/factor_apriltag_proj.h
)
SET(HDRS_FEATURE
include/apriltag/feature/feature_apriltag.h
include/apriltag/feature/feature_apriltag_proj.h
include/${PROJECT_NAME}/feature/feature_apriltag.h
include/${PROJECT_NAME}/feature/feature_apriltag_proj.h
)
SET(HDRS_LANDMARK
include/apriltag/landmark/landmark_apriltag.h
include/${PROJECT_NAME}/landmark/landmark_apriltag.h
)
SET(HDRS_PROCESSOR
include/apriltag/processor/processor_tracker_landmark_apriltag.h
include/apriltag/processor/ippe.h
include/${PROJECT_NAME}/processor/processor_tracker_landmark_apriltag.h
include/${PROJECT_NAME}/processor/ippe.h
)
# ============ SOURCES ============
......@@ -166,10 +158,8 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
# IF (Ceres_FOUND)
# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
# ENDIF(Ceres_FOUND)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfvision_LIBRARIES})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfcore)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfvision)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} apriltag)
#Build tests
......@@ -179,38 +169,55 @@ IF(BUILD_TESTS)
add_subdirectory(test)
ENDIF(BUILD_TESTS)
#install library
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
RUNTIME DESTINATION bin
LIBRARY DESTINATION ${LIB_INSTALL_DIR}
ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
# Configure the package installation
include(CMakePackageConfigHelpers)
configure_package_config_file(
${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
)
install(
FILES
${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
DESTINATION
${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
)
# Specifies include directories to use when compiling the plugin target
# This way, include_directories does not need to be called in plugins depending on this one
target_include_directories(${PLUGIN_NAME} INTERFACE
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/landmark)
INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
FILE(WRITE ${PROJECT_NAME}.found "")
INSTALL(FILES ${PROJECT_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen)
FIND_PACKAGE(Doxygen MODULE)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
......
WOLF - Windowed Localization Frames | Apriltag Plugin
===================================
WOLF Apriltag Plugin
====================
For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
For installation guide and code documentation, please visit the [documentation website](http://www.iri.upc.edu/wolf).
#edit the following line to add the librarie's header files
FIND_PATH(
wolfapriltag_INCLUDE_DIRS
NAMES apriltag.found
PATHS /usr/local/include/iri-algorithms/wolf/plugin_apriltag)
IF(wolfapriltag_INCLUDE_DIRS)
MESSAGE("Found wolf apriltag include dirs: ${wolfapriltag_INCLUDE_DIRS}")
ELSE(wolfapriltag_INCLUDE_DIRS)
MESSAGE("Couldn't find wolf apriltag include dirs")
ENDIF(wolfapriltag_INCLUDE_DIRS)
FIND_LIBRARY(
wolfapriltag_LIBRARIES
NAMES libwolfapriltag.so libwolfapriltag.dylib
PATHS /usr/local/lib)
IF(wolfapriltag_LIBRARIES)
MESSAGE("Found wolf apriltag lib: ${wolfapriltag_LIBRARIES}")
ELSE(wolfapriltag_LIBRARIES)
MESSAGE("Couldn't find wolf apriltag lib")
ENDIF(wolfapriltag_LIBRARIES)
IF (wolfapriltag_INCLUDE_DIRS AND wolfapriltag_LIBRARIES)
SET(wolfapriltag_FOUND TRUE)
ELSE(wolfapriltag_INCLUDE_DIRS AND wolfapriltag_LIBRARIES)
set(wolfapriltag_FOUND FALSE)
ENDIF (wolfapriltag_INCLUDE_DIRS AND wolfapriltag_LIBRARIES)
IF (wolfapriltag_FOUND)
IF (NOT wolfapriltag_FIND_QUIETLY)
MESSAGE(STATUS "Found wolf apriltag: ${wolfapriltag_LIBRARIES}")
ENDIF (NOT wolfapriltag_FIND_QUIETLY)
ELSE (wolfapriltag_FOUND)
IF (wolfapriltag_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find wolf apriltag")
ENDIF (wolfapriltag_FIND_REQUIRED)
ENDIF (wolfapriltag_FOUND)
macro(wolf_report_not_found REASON_MSG)
set(wolfapriltag_FOUND FALSE)
unset(wolfapriltag_INCLUDE_DIRS)
unset(wolfapriltag_LIBRARIES)
# Reset the CMake module path to its state when this script was called.
set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
# FindPackage() use the camelcase library name, not uppercase.
if (wolfapriltag_FIND_QUIETLY)
message(STATUS "Failed to find wolf apriltag- " ${REASON_MSG} ${ARGN})
else (wolfapriltag_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find wolf apriltag - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
# that prevents generation, but continues configuration.
message(SEND_ERROR "Failed to find wolf apriltag - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(wolf_report_not_found)
if(NOT wolfapriltag_FOUND)
wolf_report_not_found("Something went wrong while setting up wolf apriltag.")
endif(NOT wolfapriltag_FOUND)
# Set the include directories for wolf (itself).
set(wolfapriltag_FOUND TRUE)
# Now we gather all the required dependencies for Wolf Laser
FIND_PACKAGE(wolfvision REQUIRED)
list(APPEND wolfapriltag_INCLUDE_DIRS ${wolfvision_INCLUDE_DIR})
list(APPEND wolfapriltag_LIBRARIES ${wolfvision_LIBRARY})
if(NOT OpenCV_FOUND)
FIND_PACKAGE(OpenCV REQUIRED)
list(APPEND wolfapriltag_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
list(APPEND wolfapriltag_LIBRARIES ${OpenCV_LIBS})
endif()
#Making sure wolf is looked for
if(NOT wolf_FOUND)
FIND_PACKAGE(wolfcore REQUIRED)
#We reverse in order to insert at the start
list(REVERSE wolfapriltag_INCLUDE_DIRS)
list(APPEND wolfapriltag_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
list(REVERSE wolfapriltag_INCLUDE_DIRS)
list(REVERSE wolfapriltag_LIBRARIES)
list(APPEND wolfapriltag_LIBRARIES ${wolfcore_LIBRARIES})
list(REVERSE wolfapriltag_LIBRARIES)
endif()
# provide both INCLUDE_DIR and INCLUDE_DIRS
SET(wolfapriltag_INCLUDE_DIR ${wolfapriltag_INCLUDE_DIRS})
# provide both LIBRARY and LIBRARIES
SET(wolfapriltag_LIBRARY ${wolfapriltag_LIBRARIES})
set(@PLUGIN_NAME@_VERSION 0.0.1)
@PACKAGE_INIT@
set_and_check(@PLUGIN_NAME@_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@")
set(@PLUGIN_NAME@_INCLUDE_DIRS @PLUGIN_NAME@_INCLUDE_DIR)
set_and_check(@PLUGIN_NAME@_LIB_INSTALL_DIR "@PACKAGE_LIB_INSTALL_DIR@")
set(@PLUGIN_NAME@_LIB_INSTALL_DIRS @PLUGIN_NAME@_LIB_INSTALL_DIR)
# forwards the correct parameters given to FIND_DEPENDENCIES
include(CMakeFindDependencyMacro)
FIND_DEPENDENCY(apriltag REQUIRED)
include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
......@@ -22,16 +22,47 @@
#ifndef _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
#define _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
//Wolf includes
#include "core/common/wolf.h"
#include "core/processor/processor_tracker_landmark.h"
#include "vision/sensor/sensor_camera.h"
#include <apriltag/apriltag.h>
#include <core/factor/factor_distance_3d.h>
// Wolf apriltag includes
#include "apriltag/feature/feature_apriltag.h"
#include "apriltag/landmark/landmark_apriltag.h"
// IPPE (copy from https://github.com/tobycollins/IPPE)
#include "ippe.h"
// open cv
#include <opencv/cv.h>
// Wolf vision
#include <vision/math/pinhole_tools.h>
#include <vision/capture/capture_image.h>
#include <vision/sensor/sensor_camera.h>
// Wolf core
#include <core/math/rotations.h>
#include <core/state_block/state_quaternion.h> /// REMOVE?
#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
#include <core/processor/processor_tracker_landmark.h>
// apriltag detection Michigan library
#include <apriltag/apriltag.h>
#include <apriltag/common/homography.h>
#include <apriltag/common/zarray.h>
#include <apriltag/tag16h5.h>
#include <apriltag/tag25h9.h>
#include <apriltag/tag36h11.h>
#include <apriltag/tagCircle21h7.h>
#include <apriltag/tagCircle49h12.h>
#include <apriltag/tagCustom48h12.h>
#include <apriltag/tagStandard41h12.h>
#include <apriltag/tagStandard52h13.h>
#include "apriltag/common/homography.h"
#include "apriltag/common/zarray.h"
// opencv
// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/eigen.hpp>
// Eigen
#include <Eigen/Dense>
namespace wolf
{
......@@ -183,7 +214,9 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark
double getTagWidth(int _id) const;
std::string getTagFamily() const;
FeatureBasePtrList getIncomingDetections() const;
FeatureBasePtrList getIncomingDetections();
FeatureBasePtrList getLastDetections() const;
FeatureBasePtrList getLastDetections();
void ippePoseEstimation(apriltag_detection_t *_det,
cv::Mat_<double> _K,
double _tag_width,
......
......@@ -24,13 +24,13 @@
// which will be added to the include path for compilation,
// and installed with the public wolf headers.
#ifndef WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
#define WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
#ifndef WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
#define WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
#cmakedefine _WOLF_DEBUG
#cmakedefine _WOLF_TRACE
#define _WOLF_${UPPER_NAME}_ROOT_DIR "${_WOLF_ROOT_DIR}"
#define _WOLF_${PROJECT_NAME_UPPER}_ROOT_DIR "${_WOLF_ROOT_DIR}"
#endif /* WOLF_INTERNAL_CONFIG_H_ */
......@@ -20,37 +20,7 @@
//
//--------LICENSE_END--------
// this plugin
#include "apriltag/processor/processor_tracker_landmark_apriltag.h"
#include "apriltag/feature/feature_apriltag.h"
#include "apriltag/landmark/landmark_apriltag.h"
#include "apriltag/factor/factor_apriltag.h"
#include "apriltag/processor/ippe.h"
// dependent plugin
#include <vision/capture/capture_image.h>
#include <vision/math/pinhole_tools.h>
// wolf
#include <core/math/rotations.h>
#include <core/state_block/state_quaternion.h>
// apriltag library
#include <apriltag/common/homography.h>
#include <apriltag/common/zarray.h>
#include <apriltag/tag16h5.h>
#include <apriltag/tag25h9.h>
#include <apriltag/tag36h11.h>
#include <apriltag/tagCircle21h7.h>
#include <apriltag/tagCircle49h12.h>
#include <apriltag/tagCustom48h12.h>
#include <apriltag/tagStandard41h12.h>
#include <apriltag/tagStandard52h13.h>
#include <core/factor/factor_distance_3d.h>
// opencv
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/eigen.hpp>
namespace wolf {
......@@ -266,14 +236,11 @@ void ProcessorTrackerLandmarkApriltag::postProcess()
FactorBasePtr ProcessorTrackerLandmarkApriltag::emplaceFactor(FeatureBasePtr _feature_ptr,
LandmarkBasePtr _landmark_ptr)
{
return FactorBase::emplace<FactorApriltag>(_feature_ptr,
getSensor(),
getLast()->getFrame(),
std::static_pointer_cast<LandmarkApriltag>(_landmark_ptr),
std::static_pointer_cast<FeatureApriltag> (_feature_ptr ),
return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
_feature_ptr,
_landmark_ptr,
shared_from_this(),
params_->apply_loss_function,
FAC_ACTIVE);
params_->apply_loss_function);
}
LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr _feature_ptr)
......@@ -369,16 +336,21 @@ bool ProcessorTrackerLandmarkApriltag::voteForKeyFrame() const
if (detections_last_.empty())
return false;
double dt = getIncoming()->getTimeStamp() - getOrigin()->getTimeStamp();
double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
bool more_than_min_time_vote = dt_incoming_origin > min_time_span_;
bool too_long_since_last_KF = dt_incoming_origin > max_time_span_ + 1e-5;
// the elapsed time since last KF is too long
if (dt > max_time_span_ + 1e-5){
bool enough_features_in_last = detections_last_.size() >= min_features_for_keyframe_;
bool enough_features_in_incoming = detections_incoming_.size() >= min_features_for_keyframe_;
// not enough detection in incoming capture and a minimum time since last KF has past
if (enough_features_in_last and !enough_features_in_incoming and more_than_min_time_vote)
return true;
}
// too few detections in incoming capture and a minimum time since last KF has past
if ((detections_incoming_.size() <= min_features_for_keyframe_) and (dt >= min_time_span_))
// the elapsed time since last KF is too long
if (too_long_since_last_KF and enough_features_in_last){
return true;
}
// Vote for every image processed at the beginning if possible
if (nb_vote_ < nb_vote_for_every_first_){
......@@ -486,11 +458,27 @@ void ProcessorTrackerLandmarkApriltag::pinholeHomogeneous(Eigen::Matrix3d const
}
FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getIncomingDetections() const
{
FeatureBasePtrList list_const;
for (auto && obj : detections_incoming_)
list_const.push_back(obj);
return list_const;
}
FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getIncomingDetections()
{
return detections_incoming_;
}
FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getLastDetections() const
{
FeatureBasePtrList list_const;
for (auto && obj : detections_last_)
list_const.push_back(obj);
return list_const;
}
FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getLastDetections()
{
return detections_last_;
}
......@@ -523,25 +511,7 @@ void ProcessorTrackerLandmarkApriltag::advanceDerived()
void ProcessorTrackerLandmarkApriltag::resetDerived()
{
// Add 3d distance constraint between 2 frames
if (getProblem()->getMotionProviderMap().empty() && add_3d_cstr_){
if ((getOrigin() != nullptr) &&
(getOrigin()->getFrame() != nullptr) &&
(getOrigin() != getLast()) &&
(getOrigin()->getFrame() != getLast()->getFrame())
)
{
FrameBasePtr ori_frame = getOrigin()->getFrame();
Eigen::Vector1d dist_meas; dist_meas << 0.0;
double dist_std = 0.5;
Eigen::Matrix1d cov0(dist_std*dist_std);
auto capt3d = CaptureBase::emplace<CaptureBase>(getLast()->getFrame(),"Dist",getLast()->getTimeStamp());
auto feat_dist = FeatureBase::emplace<FeatureBase>(capt3d, "Dist", dist_meas, cov0);
auto cstr = FactorBase::emplace<FactorDistance3d>(feat_dist, feat_dist, ori_frame, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
}
// BAD -> should be rewritten some other way
if (getProblem()->getMotionProviderMap().empty() && reestimate_last_frame_){
reestimateLastFrame();
}
......
......@@ -30,7 +30,7 @@
// wolf
#include "apriltag/processor/processor_tracker_landmark_apriltag.h"
#include "core/yaml/yaml_conversion.h"
#include "core/common/factory.h"
#include "core/processor/factory_processor.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
......
# Retrieve googletest from github & compile
add_subdirectory(gtest)
# Include gtest directory.
include_directories(${GTEST_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE #################
# #
# Create a specific test executable for gtest_example #
# wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
###########################################################
wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag.cpp)
target_link_libraries(gtest_feature_apriltag ${PLUGIN_NAME})
wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp)
target_link_libraries(gtest_landmark_apriltag ${PLUGIN_NAME})
# ProcessorTrackerLandmarkApriltag test
wolf_add_gtest(gtest_processor_tracker_landmark_apriltag gtest_processor_tracker_landmark_apriltag.cpp)
target_link_libraries(gtest_processor_tracker_landmark_apriltag ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_apriltag gtest_factor_apriltag.cpp)
target_link_libraries(gtest_factor_apriltag ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_apriltag_proj gtest_factor_apriltag_proj.cpp)
target_link_libraries(gtest_factor_apriltag_proj ${PLUGIN_NAME})