From 212247a549a2a0575c39c4422302107f6e51406b Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Tue, 7 May 2019 09:16:58 +0200
Subject: [PATCH 01/58] WIP
---
CMakeLists.txt | 149 +++---------------
.../processor/processor_bundle_adjustment.h | 4 +-
src/processor/processor_bundle_adjustment.cpp | 4 +-
src/yaml/processor_bundle_adjustment_yaml.cpp | 6 +-
test/gtest_track_matrix.cpp | 2 +-
5 files changed, 30 insertions(+), 135 deletions(-)
rename include/{base => vision}/processor/processor_bundle_adjustment.h (98%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f6815c9de..faf392575 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -184,6 +184,7 @@ include/vision/processor/processor_tracker_feature_trifocal.h
include/vision/processor/processor_params_image.h
include/vision/processor/processor_tracker_landmark_image.h
include/vision/processor/processor_tracker_feature_image.h
+include/vision/processor/processor_bundle_adjustment.h
)
SET(HDRS_SENSOR
include/vision/sensor/sensor_camera.h
@@ -215,6 +216,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_tracker_feature_trifocal.cpp
src/processor/processor_tracker_feature_image.cpp
src/processor/processor_tracker_landmark_image.cpp
+src/processor/processor_bundle_adjustment.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_camera.cpp
@@ -227,136 +229,29 @@ SET(SRCS_YAML
src/yaml/processor_tracker_feature_trifocal_yaml.cpp
src/yaml/sensor_camera_yaml.cpp
src/yaml/processor_image_yaml.cpp
+src/yaml/processor_bundle_adjustment_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- include/base/solver_suitesparse/sparse_utils.h
- include/base/solver/solver_manager.h
- include/base/ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- include/base/ceres_wrapper/cost_function_wrapper.h
- include/base/ceres_wrapper/create_numeric_diff_cost_function.h
- include/base/ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- src/solver/solver_manager.cpp
- src/ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- src/ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_laser_2D.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_tracker_feature_corner.h
- include/base/processor/processor_tracker_landmark_corner.h
- )
- SET(HDRS_SENSOR ${HDRS_SENSOR}
- include/base/sensor/sensor_laser_2D.h
- )
- SET(SRCS ${SRCS}
- src/sensor/sensor_laser_2D.cpp
- src/processor/processor_tracker_feature_corner.cpp
- src/processor/processor_tracker_landmark_corner.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_GPS.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
- src/capture/capture_GPS.cpp
- src/processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_image.h
- )
- SET(HDRS_FEATURE ${HDRS_FEATURE}
- include/base/feature/feature_point_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_bundle_adjustment.h
- include/base/processor/processor_tracker_feature_trifocal.h
- include/base/processor/processor_params_image.h
- include/base/processor/processor_tracker_feature_image.h
- include/base/processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
- include/base/landmark/landmark_point_3D.h
- include/base/landmark/landmark_AHP.h
- )
- SET(SRCS ${SRCS}
- src/capture/capture_image.cpp
- src/feature/feature_point_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
- src/landmark/landmark_point_3D.cpp
- src/landmark/landmark_AHP.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_bundle_adjustment.cpp
- src/processor/processor_tracker_feature_trifocal.cpp
- src/processor/processor_tracker_feature_image.cpp
- src/processor/processor_tracker_landmark_image.cpp
- )
-ENDIF(vision_utils_FOUND)
-#SUBDIRECTORIES
-add_subdirectory(hello_wolf)
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- #DOES NOTHING?!
- #ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- include/base/yaml/yaml_conversion.h
- )
- SET(HDRS_YAML ${HDRS_YAML}
- include/base/yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- src/yaml/processor_odom_3D_yaml.cpp
- src/yaml/processor_IMU_yaml.cpp
- src/yaml/sensor_camera_yaml.cpp
- src/yaml/sensor_odom_3D_yaml.cpp
- src/yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- src/yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- src/yaml/processor_image_yaml.cpp
- src/yaml/processor_bundle_adjustment_yaml.cpp
- src/yaml/processor_tracker_feature_trifocal_yaml.cpp
- )
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
+# ==================EXAMPLE===============
+# IF (Ceres_FOUND)
+# SET(HDRS_WRAPPER
+# include/base/solver_suitesparse/sparse_utils.h
+# include/base/solver/solver_manager.h
+# include/base/ceres_wrapper/ceres_manager.h
+# include/base/ceres_wrapper/cost_function_wrapper.h
+# include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+# include/base/ceres_wrapper/local_parametrization_wrapper.h
+# )
+# SET(SRCS_WRAPPER
+# src/solver/solver_manager.cpp
+# src/ceres_wrapper/ceres_manager.cpp
+# src/ceres_wrapper/local_parametrization_wrapper.cpp
+# )
+# ELSE(Ceres_FOUND)
+# SET(HDRS_WRAPPER)
+# SET(SRCS_WRAPPER)
+# ENDIF(Ceres_FOUND)
# create the shared library
ADD_LIBRARY(${PROJECT_NAME}
diff --git a/include/base/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
similarity index 98%
rename from include/base/processor/processor_bundle_adjustment.h
rename to include/vision/processor/processor_bundle_adjustment.h
index 82e34dffd..cf07d1b0d 100644
--- a/include/base/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -9,8 +9,8 @@
#define INCLUDE_BASE_PROCESSOR_PROCESSOR_BUNDLE_ADJUSTMENT_H_
//wolf includes
-#include "base/processor/processor_tracker_feature.h"
-#include "base/capture/capture_image.h"
+#include "core/processor/processor_tracker_feature.h"
+#include "vision/capture/capture_image.h"
//vision utils includes
#include "vision_utils/vision_utils.h"
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 11d418a6c..ddffdabe9 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -13,7 +13,7 @@
//standard
#include <memory>
-#include "../../include/base/processor/processor_bundle_adjustment.h"
+#include "vision/processor/processor_bundle_adjustment.h"
namespace wolf{
@@ -193,7 +193,7 @@ ProcessorBasePtr ProcessorBundleAdjustment::create(const std::string& _unique_na
} //namespace wolf
// Register in the ProcessorFactory
-#include "base/processor/processor_factory.h"
+#include "core/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("TRACKER BUNDLE ADJUSTMENT", ProcessorBundleAdjustment)
} // namespace wolf
diff --git a/src/yaml/processor_bundle_adjustment_yaml.cpp b/src/yaml/processor_bundle_adjustment_yaml.cpp
index eaca887c9..5006e127e 100644
--- a/src/yaml/processor_bundle_adjustment_yaml.cpp
+++ b/src/yaml/processor_bundle_adjustment_yaml.cpp
@@ -6,14 +6,14 @@
*/
// wolf yaml
-#include "base/yaml/yaml_conversion.h"
+#include "core/yaml/yaml_conversion.h"
// wolf
-#include "base/common/factory.h"
+#include "core/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
-#include "../../include/base/processor/processor_bundle_adjustment.h"
+#include "vision/processor/processor_bundle_adjustment.h"
namespace wolf
{
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 454862f1d..cc56a045d 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -9,7 +9,7 @@
#include "core/processor/track_matrix.h"
-#include "base/common/time_stamp.h"
+#include "core/common/time_stamp.h"
using namespace wolf;
--
GitLab
From daec62c6d1ef6898846ab14d92c12a46033984a8 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Mon, 13 May 2019 11:31:42 +0200
Subject: [PATCH 02/58] WIP
---
.gitignore | 2 +
CMakeLists.txt | 3 +
include/vision/factor/factor_pixelHP.h | 184 +++++
include/vision/landmark/landmark_HP.h | 72 ++
.../processor/processor_bundle_adjustment.h | 36 +-
src/CMakeLists.txt | 714 ------------------
src/examples/processor_bundle_adjustment.yaml | 2 +-
src/landmark/landmark_HP.cpp | 74 ++
src/processor/processor_bundle_adjustment.cpp | 231 +++++-
src/yaml/processor_bundle_adjustment_yaml.cpp | 16 +-
test/CMakeLists.txt | 6 +-
test/gtest_processor_bundle_adjustment.cpp | 80 ++
test/gtest_sensor_camera.cpp | 26 +-
13 files changed, 676 insertions(+), 770 deletions(-)
create mode 100644 include/vision/factor/factor_pixelHP.h
create mode 100644 include/vision/landmark/landmark_HP.h
delete mode 100644 src/CMakeLists.txt
create mode 100644 src/landmark/landmark_HP.cpp
create mode 100644 test/gtest_processor_bundle_adjustment.cpp
diff --git a/.gitignore b/.gitignore
index d683ba3c8..8c8bcaa25 100644
--- a/.gitignore
+++ b/.gitignore
@@ -29,3 +29,5 @@ src/CMakeCache.txt
src/CMakeFiles/cmake.check_cache
src/examples/map_apriltag_save.yaml
+
+vision.found
diff --git a/CMakeLists.txt b/CMakeLists.txt
index faf392575..425193f81 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -172,12 +172,14 @@ include/vision/capture/capture_image.h
SET(HDRS_FACTOR
include/vision/factor/factor_autodiff_trifocal.h
include/vision/factor/factor_AHP.h
+include/vision/factor/factor_pixelHP.h
)
SET(HDRS_FEATURE
include/vision/feature/feature_point_image.h
)
SET(HDRS_LANDMARK
include/vision/landmark/landmark_AHP.h
+ include/vision/landmark/landmark_HP.h
)
SET(HDRS_PROCESSOR
include/vision/processor/processor_tracker_feature_trifocal.h
@@ -211,6 +213,7 @@ src/feature/feature_point_image.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_AHP.cpp
+src/landmark/landmark_HP.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_tracker_feature_trifocal.cpp
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
new file mode 100644
index 000000000..070ee1062
--- /dev/null
+++ b/include/vision/factor/factor_pixelHP.h
@@ -0,0 +1,184 @@
+#ifndef FACTOR_AHP_H
+#define FACTOR_AHP_H
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "vision/landmark/landmark_HP.h"
+#include "vision/sensor/sensor_camera.h"
+//#include "core/feature/feature_point_image.h"
+#include "core/math/pinhole_tools.h"
+
+#include <iomanip> //setprecision
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPixelHP);
+
+//class
+class FactorPixelHP : public FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>
+{
+ protected:
+
+ Eigen::Vector4s intrinsic_;
+ Eigen::VectorXs distortion_;
+
+ public:
+
+ FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
+ virtual ~FactorPixelHP() = default;
+
+ template<typename T>
+ void expectation(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _expectation) const;
+
+ Eigen::VectorXs expectation() const;
+
+ template<typename T>
+ bool operator ()(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _residuals) const;
+
+ // Static creator method
+ static FactorPixelHPPtr create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
+};
+
+inline FactorPixelHP::FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>("PIXELHP",
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getCapture()->getFrame()->getP(),
+ _ftr_ptr->getCapture()->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO(),
+ _landmark_ptr->getP()),
+ intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState()) //TODO: what to do with intrinsic?
+{
+ // obtain some intrinsics from provided sensor
+ distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
+}
+
+inline Eigen::VectorXs FactorPixelHP::expectation() const
+{
+ FrameBasePtr frm = getFeature()->getCapture()->getFrame();
+ SensorBasePtr sen = getFeature()->getCapture()->getSensor();
+ LandmarkBasePtr lmk = getLandmarkOther();
+
+ const Eigen::MatrixXs frame_pos = frm->getP()->getState();
+ const Eigen::MatrixXs frame_ori = frm->getO()->getState();
+ const Eigen::MatrixXs sensor_pos = sen ->getP()->getState();
+ const Eigen::MatrixXs sensor_ori = sen ->getO()->getState();
+ const Eigen::MatrixXs lmk_pos_hmg = lmk ->getP()->getState();
+
+ Eigen::Vector2s exp;
+ expectation(frame_pos.data(), frame_ori.data(), sensor_pos.data(), sensor_ori.data(),
+ lmk_pos_hmg.data(), exp.data());
+
+ return exp;
+}
+
+template<typename T>
+inline void FactorPixelHP::expectation(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _expectation) const
+{
+ using namespace Eigen;
+
+ // All involved transforms typedef
+ typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
+
+ // world to current robot transform
+ Map<const Matrix<T, 3, 1> > p_w_r(_frame_p);
+ Translation<T, 3> t_w_r(p_w_r);
+ Map<const Quaternion<T> > q_w_r(_frame_o);
+ TransformType T_W_R = t_w_r * q_w_r;
+
+ // current robot to current camera transform
+ CaptureBasePtr capture = this->getFeature()->getCapture();
+ Translation<T, 3> t_r_c (capture->getSensorP()->getState().cast<T>());
+ Quaternions q_r_c_s(Eigen::Vector4s(capture->getSensorO()->getState()));
+ Quaternion<T> q_r_c = q_r_c_s.cast<T>();
+ TransformType T_R_C = t_r_c * q_r_c;
+
+ // hmg point in current camera frame C
+ Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg(_lmk_hmg);
+ Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_R_C .inverse(Eigen::Affine)
+ * T_W_R .inverse(Eigen::Affine)
+ * landmark_hmg;
+
+ // lmk direction vector //TODO:
+ Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c.head(3);
+
+ // camera parameters
+ Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
+ Eigen::Matrix<T, Eigen::Dynamic, 1> distortion = distortion_.cast<T>();
+
+ // project point and exit
+ Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
+ expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
+}
+
+template<typename T>
+inline bool FactorPixelHP::operator ()(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _residuals) const
+{
+ // expected
+ Eigen::Matrix<T, 2, 1> expected;
+ expectation(_frame_p, _frame_o, _sensor_p, _sensor_o, _lmk_hmg, expected.data());
+
+ // measured
+ Eigen::Matrix<T, 2, 1> measured = getMeasurement().cast<T>();
+
+ // residual
+ Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected - measured);
+ return true;
+}
+
+inline FactorPixelHPPtr FactorPixelHP::create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+{
+ // construct factor
+ FactorPixelHPPtr fac_ahp = std::make_shared<FactorPixelHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
+
+ return fac_ahp;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_AHP_H
diff --git a/include/vision/landmark/landmark_HP.h b/include/vision/landmark/landmark_HP.h
new file mode 100644
index 000000000..cea8c04bc
--- /dev/null
+++ b/include/vision/landmark/landmark_HP.h
@@ -0,0 +1,72 @@
+#ifndef LANDMARK_AHP_H
+#define LANDMARK_AHP_H
+
+//Wolf includes
+#include "core/landmark/landmark_base.h"
+
+// yaml
+#include <yaml-cpp/yaml.h>
+
+// Vision utils
+#include <vision_utils/vision_utils.h>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(LandmarkHP);
+
+/* Landmark - Homogeneous Point*/
+class LandmarkHP : public LandmarkBase
+{
+ protected:
+ SensorBasePtr sensor_;
+ cv::Mat cv_descriptor_;
+
+
+ public:
+ LandmarkHP(Eigen::Vector4s _position_homogeneous, SensorBasePtr _sensor_, cv::Mat _2D_descriptor);
+
+ virtual ~LandmarkHP();
+
+ const cv::Mat& getCvDescriptor() const;
+ void setCvDescriptor(const cv::Mat& _descriptor);
+
+ void setSensor (SensorBasePtr _anchor_sensor);
+ const SensorBasePtr getSensor() const;
+
+ Eigen::Vector3s point() const;
+
+ YAML::Node saveToYaml() const;
+
+ /** \brief Creator for Factory<LandmarkBase, YAML::Node>
+ * Caution: This creator does not set the landmark's sensor.
+ * These need to be set afterwards.
+ */
+ static LandmarkBasePtr create(const YAML::Node& _node);
+};
+
+inline void LandmarkHP::setSensor(SensorBasePtr _sensor)
+{
+ sensor_ = _sensor;
+}
+
+
+inline const SensorBasePtr LandmarkHP::getSensor() const
+{
+ return sensor_;
+}
+
+
+inline const cv::Mat& LandmarkHP::getCvDescriptor() const
+{
+ return cv_descriptor_;
+}
+
+
+inline void LandmarkHP::setCvDescriptor(const cv::Mat& _descriptor)
+{
+ cv_descriptor_ = _descriptor;
+}
+
+} // namespace wolf
+
+#endif // LANDMARK_AHP_H
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index cf07d1b0d..11e9b371b 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -1,5 +1,5 @@
/*
- * processor_tracker_feature_oriol.h
+ * processor_bundle_adjustment.h
*
* Created on: May 3, 2019
* Author: ovendrell
@@ -11,12 +11,14 @@
//wolf includes
#include "core/processor/processor_tracker_feature.h"
#include "vision/capture/capture_image.h"
+#include "vision/landmark/landmark_HP.h"
//vision utils includes
#include "vision_utils/vision_utils.h"
#include "vision_utils/detectors.h"
#include "vision_utils/descriptors.h"
#include "vision_utils/matchers.h"
+#include "../factor/factor_pixelHP.h"
namespace wolf{
@@ -26,6 +28,10 @@ struct ProcessorParamsBundleAdjustment : public ProcessorParamsTrackerFeature
{
std::string yaml_file_params_vision_utils;
+ bool delete_ambiguities;
+
+ Scalar pixel_noise_std; ///< std noise of the pixel
+ int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor
};
@@ -38,20 +44,25 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
vision_utils::DescriptorBasePtr des_ptr_;
vision_utils::MatcherBasePtr mat_ptr_;
- ProcessorParamsBundleAdjustmentPtr params_tracker_feature_oriol_; // Configuration parameters
+ ProcessorParamsBundleAdjustmentPtr params_bundle_adjustment_; // Configuration parameters
CaptureImagePtr capture_image_last_;
CaptureImagePtr capture_image_incoming_;
Matrix2s pixel_cov_;
+
+ //TODO: correct to add this?
+ std::map<size_t, LandmarkBasePtr> lmk_track_map_; //LandmarkTrackMap;
+
+
public:
/** \brief Class constructor
*/
- ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_tracker_feature_oriol);
+ ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_bundle_adjustment);
/** \brief Class destructor
*/
- virtual ~ProcessorBundleAdjustment();
+ //virtual ~ProcessorBundleAdjustment();
public:
@@ -127,11 +138,28 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
*/
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+
+ /** \brief Establish factors between features in Captures \b last and \b origin
+ */
+ virtual void establishFactors() override;
+
public:
/// @brief Factory method
static ProcessorBasePtr create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr);
+
+ private:
+
+ cv::Mat image_debug_;
+
+ public:
+
+ /**
+ * \brief Return Image for debug purposes
+ */
+ cv::Mat getImageDebug();
};
} //namespace wolf
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index c64df8aaa..000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,714 +0,0 @@
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#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)
-ENDIF()
-
-option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-
-option(BUILD_EXAMPLES "Build examples" ON)
-set(BUILD_TESTS true)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-#find dependencies.
-
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
-
-FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
- FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
-IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
-ENDIF(laser_scan_utils_FOUND)
-
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
-IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
-# YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
-IF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
- MESSAGE("glog Library FOUND: glog related sources will be built.")
- MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
- MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
- MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
- Suitesparse_INCLUDE_DIRS
- NAMES SuiteSparse_config.h
- PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND TRUE)
- MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND FALSE)
- MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
-
-# Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
-
-# Create the specified output directory if it does not exist.
-IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
- message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
- file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
-ENDIF()
-IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
- message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
- "${WOLF_CONFIG_DIR} exists, but is not a directory.")
-ENDIF()
-# Configure config.h
-configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-
-#TEMPORAL INCLUDE UNTIL WE FIGURE OUT WHAT TO DO WITH FILES in src/temp
-
-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-include_directories(../dummyInclude)
-
-# Include config.h directory at first.
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-INCLUDE_DIRECTORIES(.)
-
-# include spdlog (logging library)
-FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
-IF (SPDLOG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
- MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
-ELSE (SPDLOG_INCLUDE_DIR)
- MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)
-
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
-# cereal
-IF(cereal_FOUND)
- INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
- INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
-IF(GLOG_FOUND)
-INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
-#headers
-SET(HDRS_BASE
-capture/capture_motion.h
- eigen_assert.h
- eigen_predicates.h
-landmark/landmark_match.h
- make_unique.h
- pinhole_tools.h
-processor/processor_capture_holder.h
-processor/processor_tracker_landmark.h
- )
-SET(HDRS
-capture/capture_motion.h
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_AHP.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- temp/diff_drive_tools.h
- temp/diff_drive_tools.hpp
-feature/feature_corner_2D.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- IMU_tools.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- local_parametrization_polyline_extreme.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
- test/processor_IMU_UnitTester.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark_dummy.h
-sensor/sensor_camera.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
- SET(HDRS_CAPTURE
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-capture/capture_velocity.h
-capture/capture_wheel_joint_position.h
- )
- SET(HDRS_CONSTRAINT
-factor/factor_autodiff_trifocal.h
-factor/factor_autodiff_distance_3D.h
-factor/factor_AHP.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_diff_drive.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- )
- SET(HDRS_FEATURE
-feature/feature_corner_2D.h
-feature/feature_diff_drive.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- )
- SET(HDRS_LANDMARK
-landmark/landmark_match.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- )
- SET(HDRS_PROCESSOR
-processor/processor_capture_holder.h
-processor/processor_diff_drive.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark.h
-processor/processor_tracker_landmark_dummy.h
- )
- SET(HDRS_SENSOR
-sensor/sensor_camera.h
-sensor/sensor_diff_drive.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
-# [Add generic derived header before this line]
-
-SET(HDRS_DTASSC
- data_association/matrix.h
- data_association/association_solver.h
- data_association/association_node.h
- data_association/association_tree.h
- data_association/association_nnls.h
- )
-
-SET(HDRS_CORE
-core/capture_base.h
-core/capture_buffer.h
-core/capture_pose.h
-core/capture_void.h
-core/factor_analytic.h
-core/factor_autodiff.h
-core/factor_base.h
-core/factory.h
-core/feature_base.h
-core/feature_match.h
-core/feature_pose.h
-core/frame_base.h
-core/hardware_base.h
-core/landmark_base.h
-core/local_parametrization_angle.h
-core/local_parametrization_base.h
-core/local_parametrization_homogeneous.h
-core/local_parametrization_quaternion.h
-core/logging.h
-core/map_base.h
-core/motion_buffer.h
-core/node_base.h
-core/problem.h
-core/processor_base.h
-core/factory.h
-core/processor_loopclosure_base.h
-core/processor_motion.h
-core/processor_tracker_feature.h
-core/processor_tracker.h
-core/rotations.h
-core/sensor_base.h
-core/factory.h
-core/singleton.h
-core/state_angle.h
-core/state_block.h
-core/state_homogeneous_3D.h
-core/state_quaternion.h
-core/three_D_tools.h
-core/time_stamp.h
-core/track_matrix.h
-core/trajectory_base.h
-core/wolf.h
- )
-SET(SRCS_CORE
-core/capture_base.cpp
-core/capture_pose.cpp
-core/capture_void.cpp
-core/factor_analytic.cpp
-core/factor_base.cpp
-core/feature_base.cpp
-core/feature_pose.cpp
-core/frame_base.cpp
-core/hardware_base.cpp
-core/landmark_base.cpp
-core/local_parametrization_base.cpp
-core/local_parametrization_homogeneous.cpp
-core/local_parametrization_quaternion.cpp
-core/map_base.cpp
-core/motion_buffer.cpp
-core/node_base.cpp
-core/problem.cpp
-core/processor_base.cpp
-core/processor_loopclosure_base.cpp
-core/processor_motion.cpp
-core/processor_tracker.cpp
-core/processor_tracker_feature.cpp
-core/sensor_base.cpp
-core/state_block.cpp
-core/time_stamp.cpp
-core/track_matrix.cpp
-core/trajectory_base.cpp
- )
-#sources
-SET(SRCS_BASE
-capture/capture_motion.cpp
-processor/processor_capture_holder.cpp
-processor/processor_tracker_landmark.cpp
- )
-
-SET(SRCS
- local_parametrization_polyline_extreme.cpp
- test/processor_IMU_UnitTester.cpp
- )
- SET(SRCS_CAPTURE
-capture/capture_GPS_fix.cpp
-capture/capture_IMU.cpp
-capture/capture_odom_2D.cpp
-capture/capture_odom_3D.cpp
-capture/capture_velocity.cpp
-capture/capture_wheel_joint_position.cpp
- )
- SET(SRCS_FEATURE
-feature/feature_corner_2D.cpp
-feature/feature_diff_drive.cpp
-feature/feature_GPS_fix.cpp
-feature/feature_GPS_pseudorange.cpp
-feature/feature_IMU.cpp
-feature/feature_odom_2D.cpp
-feature/feature_polyline_2D.cpp
- )
- SET(SRCS_LANDMARK
-landmark/landmark_corner_2D.cpp
-landmark/landmark_container.cpp
-landmark/landmark_line_2D.cpp
-landmark/landmark_polyline_2D.cpp
- )
- SET(SRCS_PROCESSOR
-processor/processor_frame_nearest_neighbor_filter.cpp
-processor/processor_diff_drive.cpp
-processor/processor_IMU.cpp
-processor/processor_odom_2D.cpp
-processor/processor_odom_3D.cpp
-processor/processor_tracker_feature_dummy.cpp
-processor/processor_tracker_landmark_dummy.cpp
- )
- SET(SRCS_SENSOR
-sensor/sensor_camera.cpp
-sensor/sensor_diff_drive.cpp
-sensor/sensor_GPS.cpp
-sensor/sensor_GPS_fix.cpp
-sensor/sensor_IMU.cpp
-sensor/sensor_odom_2D.cpp
-sensor/sensor_odom_3D.cpp
- )
-SET(SRCS_DTASSC
- data_association/association_solver.cpp
- data_association/association_node.cpp
- data_association/association_tree.cpp
- data_association/association_nnls.cpp
- )
-
-# Add the solver sub-directory
-add_subdirectory(solver)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- ceres_wrapper/sparse_utils.h
- #ceres_wrapper/solver_manager.h
- ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- ceres_wrapper/cost_function_wrapper.h
- ceres_wrapper/create_numeric_diff_cost_function.h
- ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- #ceres_wrapper/solver_manager.cpp
- ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_laser_2D.h
-sensor/sensor_laser_2D.h
-processor/processor_tracker_feature_corner.h
-processor/processor_tracker_landmark_corner.h
-processor/processor_tracker_landmark_polyline.h
- )
- SET(SRCS ${SRCS}
-capture/capture_laser_2D.cpp
-sensor/sensor_laser_2D.cpp
-processor/processor_tracker_feature_corner.cpp
-processor/processor_tracker_landmark_corner.cpp
-processor/processor_tracker_landmark_polyline.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_GPS.h
-processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
-capture/capture_GPS.cpp
-processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_image.h
-feature/feature_point_image.h
-landmark/landmark_AHP.h
-processor/processor_params_image.h
-processor/processor_tracker_feature_image.h
-processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
-landmark/landmark_point_3D.h
- )
- SET(SRCS ${SRCS}
-capture/capture_image.cpp
-feature/feature_point_image.cpp
-landmark/landmark_AHP.cpp
-processor/processor_tracker_feature_image.cpp
-processor/processor_tracker_landmark_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
-landmark/landmark_point_3D.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.cpp
- )
-ENDIF(vision_utils_FOUND)
-
-# Add the capture sub-directory
-# ADD_SUBDIRECTORY(captures)
-
-# Add the factors sub-directory
-# ADD_SUBDIRECTORY(factors)
-
-# Add the features sub-directory
-# ADD_SUBDIRECTORY(features)
-
-# Add the landmarks sub-directory
-# ADD_SUBDIRECTORY(landmarks)
-
-# Add the processors sub-directory
-# ADD_SUBDIRECTORY(processors)
-
-# Add the sensors sub-directory
-# ADD_SUBDIRECTORY(sensors)
-
-# Add the hello_wolf sub-directory
-ADD_SUBDIRECTORY(hello_wolf)
-
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- yaml/processor_odom_3D_yaml.cpp
- yaml/processor_IMU_yaml.cpp
- yaml/sensor_camera_yaml.cpp
- yaml/sensor_odom_3D_yaml.cpp
- yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_image_yaml.cpp
- yaml/processor_tracker_feature_trifocal_yaml.cpp
- )
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
-
-# create the shared library
-add_library(${PROJECT_NAME}_core SHARED
- ${SRCS_CORE}
- ${SRCS_BASE}
- )
-ADD_LIBRARY(${PROJECT_NAME}
- SHARED
- ${SRCS_BASE}
- ${SRCS}
- ${SRCS_CAPTURE}
- ${SRCS_CONSTRAINT}
- ${SRCS_FEATURE}
- ${SRCS_LANDMARK}
- ${SRCS_PROCESSOR}
- ${SRCS_SENSOR}
- #${SRCS_DTASSC}
- ${SRCS_SOLVER}
- ${SRCS_WRAPPER}
- )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
-
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
-#Link the created libraries
-#=============================================================
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
-
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
-
-#install library
-install(TARGETS ${PROJECT_NAME}_core DESTINATION lib/iri-algorithms EXPORT ${PROJECT_NAME}_core-targets)
-install(EXPORT ${PROJECT_NAME}_core-targets DESTINATION lib/iri-algorithms)
-#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
-
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
-
-#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS_CORE}
- DESTINATION include/iri-algorithms/wolf/core)
-#INSTALL(FILES ${HDRS_DTASSC}
-# DESTINATION include/iri-algorithms/wolf/data_association)
-INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/capture)
-INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/factor)
-INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/serialization)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
-
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/internal)
-
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
-export(PACKAGE ${PROJECT_NAME})
-
-#############
-## Testing ##
-#############
-IF (GLOG_FOUND)
- IF(BUILD_TESTS)
- MESSAGE("Building tests.")
- add_subdirectory(test)
- ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
-
-IF(BUILD_EXAMPLES)
- #Build examples
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(examples)
-ENDIF(BUILD_EXAMPLES)
-
diff --git a/src/examples/processor_bundle_adjustment.yaml b/src/examples/processor_bundle_adjustment.yaml
index dfe322d21..788373081 100644
--- a/src/examples/processor_bundle_adjustment.yaml
+++ b/src/examples/processor_bundle_adjustment.yaml
@@ -12,7 +12,7 @@ algorithm:
grid horiz cells: 13
grid vert cells: 10
min response new features: 50
- min track length for constraint: 3
+ min track length for factor: 3
noise:
pixel noise std: 1 # pixels
diff --git a/src/landmark/landmark_HP.cpp b/src/landmark/landmark_HP.cpp
new file mode 100644
index 000000000..73cfc148d
--- /dev/null
+++ b/src/landmark/landmark_HP.cpp
@@ -0,0 +1,74 @@
+#include "vision/landmark/landmark_HP.h"
+
+#include "core/state_block/state_homogeneous_3D.h"
+#include "core/common/factory.h"
+#include "core/yaml/yaml_conversion.h"
+
+namespace wolf {
+
+/* Landmark - Anchored Homogeneous Point*/
+LandmarkHP::LandmarkHP(Eigen::Vector4s _position_homogeneous,
+ SensorBasePtr _sensor,
+ cv::Mat _2D_descriptor) :
+ LandmarkBase("HP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
+ sensor_(_sensor),
+ cv_descriptor_(_2D_descriptor.clone())
+{
+}
+
+LandmarkHP::~LandmarkHP()
+{
+ //
+}
+
+YAML::Node LandmarkHP::saveToYaml() const
+{
+ // First base things
+ YAML::Node node = LandmarkBase::saveToYaml();
+
+ // Then add specific things
+ std::vector<int> v;
+ LandmarkHP::cv_descriptor_.copyTo(v);
+ node["descriptor"] = v;
+ return node;
+}
+
+Eigen::Vector3s LandmarkHP::point() const
+{
+ using namespace Eigen;
+
+ /* TODO:
+ Transform<Scalar,3,Affine> T_w_r
+ = Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
+ * Quaternions(getAnchorFrame()->getO()->getState().data());
+ Transform<Scalar,3,Affine> T_r_c
+ = Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
+ * Quaternions(getAnchorSensor()->getO()->getState().data());
+ */
+ //Vector4s point_hmg_c = getP()->getState();
+ //Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
+
+ Vector4s point_hmg = getP()->getState();
+
+ return point_hmg.head<3>()/point_hmg(3);
+}
+
+LandmarkBasePtr LandmarkHP::create(const YAML::Node& _node)
+{
+ unsigned int id = _node["id"] .as< unsigned int >();
+ Eigen::VectorXs pos_homog = _node["position"] .as< Eigen::VectorXs >();
+ std::vector<int> v = _node["descriptor"] .as< std::vector<int> >();
+ cv::Mat desc(v);
+
+ LandmarkBasePtr lmk = std::make_shared<LandmarkHP>(pos_homog, nullptr, desc);
+ lmk->setId(id);
+ return lmk;
+}
+
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_ahp = LandmarkFactory::get().registerCreator("HP", LandmarkHP::create);
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index ddffdabe9..c39ee19e4 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -1,5 +1,5 @@
/*
- * processor_tracker_feature_oriol.cpp
+ * processor_bundle_adjustment.cpp
*
* Created on: May 3, 2019
* Author: ovendrell
@@ -17,27 +17,29 @@
namespace wolf{
-ProcessorBundleAdjustment::ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_tracker_feature_oriol) :
- ProcessorTrackerFeature("TRACKER BUNDLE ADJUSTMENT", _params_tracker_feature_oriol),
- params_tracker_feature_oriol_(_params_tracker_feature_oriol),
+ProcessorBundleAdjustment::ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_bundle_adjustment) :
+ ProcessorTrackerFeature("TRACKER BUNDLE ADJUSTMENT", _params_bundle_adjustment),
+ params_bundle_adjustment_(_params_bundle_adjustment),
capture_image_last_(nullptr),
capture_image_incoming_(nullptr)
{
+ pixel_cov_ = Eigen::Matrix2s::Identity() * params_bundle_adjustment_->pixel_noise_std * params_bundle_adjustment_->pixel_noise_std;
+
// Detector yaml file
- std::string det_name = vision_utils::readYamlType(params_tracker_feature_oriol_->yaml_file_params_vision_utils, "detector");
+ std::string det_name = vision_utils::readYamlType(params_bundle_adjustment_->yaml_file_params_vision_utils, "detector");
// Create Detector
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_feature_oriol_->yaml_file_params_vision_utils);
+ det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_bundle_adjustment_->yaml_file_params_vision_utils);
// Descriptor yaml file
- std::string des_name = vision_utils::readYamlType(params_tracker_feature_oriol_->yaml_file_params_vision_utils, "descriptor");
+ std::string des_name = vision_utils::readYamlType(params_bundle_adjustment_->yaml_file_params_vision_utils, "descriptor");
// Create Descriptor
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_feature_oriol_->yaml_file_params_vision_utils);
+ des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_bundle_adjustment_->yaml_file_params_vision_utils);
// Matcher yaml file
- std::string mat_name = vision_utils::readYamlType(params_tracker_feature_oriol_->yaml_file_params_vision_utils, "matcher");
+ std::string mat_name = vision_utils::readYamlType(params_bundle_adjustment_->yaml_file_params_vision_utils, "matcher");
// Create Matcher
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_oriol_->yaml_file_params_vision_utils);
+ mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_bundle_adjustment_->yaml_file_params_vision_utils);
}
@@ -65,18 +67,65 @@ void ProcessorBundleAdjustment::preProcess()
capture_image_incoming_->matches_normalized_scores_ = mat_ptr_->robustMatch(capture_image_incoming_->keypoints_, capture_image_last_->keypoints_,
capture_image_incoming_->descriptors_, capture_image_last_->descriptors_,
des_ptr_->getSize(), capture_image_incoming_->matches_from_precedent_);
- // Set capture map of match indices
- for (auto match : capture_image_incoming_->matches_from_precedent_)
+ //TODO: Get only the best ones
+ if (params_bundle_adjustment_->delete_ambiguities) //filter ambiguities
+ {
+ std::map<int,int> ambiguities_map;
+ for (auto match : capture_image_incoming_->matches_from_precedent_)
+ if (ambiguities_map.count(match.trainIdx))
+ ambiguities_map[match.trainIdx] = 0;
+ else
+ ambiguities_map[match.trainIdx] = 1;
+ // Set capture map of match indices
+ for (auto match : capture_image_incoming_->matches_from_precedent_)
+ {
+ if (!ambiguities_map.at(match.trainIdx))
+ capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
+ }
+ }
+ else
{
- capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
- //TODO: Nomes es queda l'ultim si hi ha ambiguitat?
+ // Set capture map of match indices
+ for (auto match : capture_image_incoming_->matches_from_precedent_)
+ capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
+
}
}
}
void ProcessorBundleAdjustment::postProcess()
{
+ //TODO: visualization
+
+ std::map<int,std::list<vision_utils::KeyPointEnhanced> > kp_enh_tracks;
+
+ for (auto const & feat_base : last_ptr_->getFeatureList())
+ {
+ FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ unsigned int feat_id = feat->id();
+ unsigned int track_id = feat->trackId();
+ // tracks
+ std::list<vision_utils::KeyPointEnhanced> kp_enh_track;
+ for (auto feat_base : track_matrix_.trackAsVector(track_id))
+ {
+ FeaturePointImagePtr feat_img = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ vision_utils::KeyPointEnhanced kp_enh(feat_img->getKeypoint(),
+ feat_id,
+ track_id,
+ track_matrix_.trackSize(track_id),
+ feat_img->getMeasurementCovariance());
+ kp_enh_track.push_back(kp_enh);
+ }
+
+ kp_enh_tracks[feat_id] = kp_enh_track;
+ }
+
+ // DEBUG
+ image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks);
+
+ cv::imshow("DEBUG VIEW", image_debug_);
+ cv::waitKey(1);
}
@@ -84,11 +133,11 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
{
for (auto feat_base: _features_last_in)
{
- FeaturePointImagePtr feat_last_ = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ FeaturePointImagePtr feat_last = std::static_pointer_cast<FeaturePointImage>(feat_base);
- if (capture_image_last_->map_index_to_next_.count(feat_last_->getIndexKeyPoint())) //If the track exists
+ if (capture_image_last_->map_index_to_next_.count(feat_last->getIndexKeyPoint())) //If the track exists
{
- int index_inc = capture_image_last_->map_index_to_next_.at(feat_last_->getIndexKeyPoint());
+ int index_inc = capture_image_last_->map_index_to_next_.at(feat_last->getIndexKeyPoint());
if (capture_image_incoming_->matches_normalized_scores_.at(index_inc) > mat_ptr_->getParams()->min_norm_score )
{
@@ -96,19 +145,19 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
cv::KeyPoint kp_inc = capture_image_incoming_->keypoints_.at(index_inc);
//cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(feat_last_->getIndexKeyPoint());
- FeaturePointImagePtr feat_inc_ = std::make_shared<FeaturePointImage>(kp_inc, index_inc,
+ FeaturePointImagePtr feat_inc = std::make_shared<FeaturePointImage>(kp_inc, index_inc,
capture_image_incoming_->descriptors_.row(index_inc),
pixel_cov_);
- _features_incoming_out.push_back(feat_inc_);
+ _features_incoming_out.push_back(feat_inc);
- //TODO: Check ambiguities?? Aqui no
+ auto feature_match_ptr = std::make_shared<FeatureMatch>();
+ feature_match_ptr->feature_ptr_= feat_last;
+ feature_match_ptr->normalized_score_ = capture_image_incoming_->matches_normalized_scores_.at(index_inc);
+ _feature_correspondences[feat_inc] = feature_match_ptr;
- _feature_correspondences[feat_inc_] = std::make_shared<FeatureMatch>(feat_last_,
- capture_image_incoming_->matches_normalized_scores_.at(index_inc));
}
}
-
}
return _feature_correspondences.size();
@@ -131,18 +180,16 @@ bool ProcessorBundleAdjustment::is_tracked(int& _kp_idx)
unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
{
- /*TODO: Implement
- * Detect features again : NO fa falta
- * Filter to get only the new features compared to the ones we had tracked before
- * \param _features_last_out is new_last_features que s'ha d'omplir
- * tenir en compte que es fa trackfeatures amb el resultat i no s'han de perdre els tracks ja obtinguts (i si hi ha indexs duplicats?)
- */
+ //TODO: efficient implementation?
typedef std::map<int,int>::iterator iter;
for (iter it = capture_image_last_->map_index_to_next_.begin(); it!=capture_image_last_->map_index_to_next_.end(); ++it)
{
- if(!is_tracked(it->second))
+ if (_features_last_out.size() >= _max_new_features)
+ break;
+
+ else if(!is_tracked(it->second))
{
//add to _features_last_out
int idx_last = it->first;
@@ -151,6 +198,7 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
capture_image_last_->descriptors_.row(idx_last),
pixel_cov_);
_features_last_out.push_back(feat_last_);
+
}
}
@@ -158,25 +206,132 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
}
-bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
+bool ProcessorBundleAdjustment::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
{
//TODO: Implement
return false;
}
-bool voteForKeyFrame()
+bool ProcessorBundleAdjustment::voteForKeyFrame()
{
- //TODO: Implement
- return false;
+ // // A. crossing voting threshold with ascending number of features
+ bool vote_up = false;
+ // // 1. vote if we did not have enough features before
+ // vote_up = vote_up && (previousNumberOfTracks() < params_bundle_adjustment_->min_features_for_keyframe);
+ // // 2. vote if we have enough features now
+ // vote_up = vote_up && (incoming_ptr_->getFeatureList().size() >= params_bundle_adjustment_->min_features_for_keyframe);
+
+ // B. crossing voting threshold with descending number of features
+ bool vote_down = true;
+ // 1. vote if we had enough features before
+ // vote_down = vote_down && (last_ptr_->getFeatureList().size() >= params_bundle_adjustment_->min_features_for_keyframe);
+ // 2. vote if we have not enough features now
+ vote_down = vote_down && (incoming_ptr_->getFeatureList().size() < params_bundle_adjustment_->min_features_for_keyframe);
+
+ // // C. Time-based policies
+ bool vote_time = false;
+ //// vote_time = vote_time || (incoming_ptr_->getTimeStamp()-origin_ptr_->getTimeStamp() > 1.0);
+ //
+ // if (vote_up)
+ // WOLF_TRACE("VOTE UP");
+ // if (vote_down)
+ // WOLF_TRACE("VOTE DOWN");
+ // if (vote_time)
+ // WOLF_TRACE("VOTE TIME");
+
+ return vote_up || vote_down || vote_time;
}
-FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+
+FactorBasePtr ProcessorBundleAdjustment::createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
{
- //TODO: Implement
- FactorBasePtr a;
- return a;
+ /* This function is no creating any factor.
+ * We create factors with establishFactors()
+ */
+ return FactorBasePtr();
+}
+
+LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _feature_ptr)
+{
+
+ FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
+ //FrameBasePtr anchor_frame = getLast()->getFrame();
+
+ Eigen::Vector2s point2D;
+ point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
+ point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
+
+ //Scalar distance = params_bundle_adjustment_->distance; // arbitrary value
+ Eigen::Vector4s vec_homogeneous;
+
+ point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
+ point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
+
+ Eigen::Vector3s point3D;
+ point3D.head<2>() = point2D;
+ point3D(2) = 1;
+
+ point3D.normalize();
+
+ vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/1};//distance};
+
+ LandmarkHPPtr lmk_hp_ptr = std::make_shared<LandmarkHP>(vec_homogeneous, getSensor(), feat_point_image_ptr->getDescriptor());
+ _feature_ptr->setLandmarkId(lmk_hp_ptr->id());
+ return lmk_hp_ptr;
}
+void ProcessorBundleAdjustment::establishFactors()
+{
+
+ TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, incoming_ptr_);
+
+ for (auto const & pair_trkid_pair : matches_origin_inc)
+ {
+ size_t trkid = pair_trkid_pair.first;
+ FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
+ FeatureBasePtr feature_inc = pair_trkid_pair.second.second;
+
+ //TODO: don't use map[track_id] = landmarkptr.
+ if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
+ {
+ //createLandmark
+ LandmarkBasePtr lmk = createLandmark(feature_origin);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+ //add Landmark to map: map[track_id] = landmarkptr
+ lmk_track_map_[trkid] = lmk;
+
+ //create factors
+ FactorPixelHPPtr fac_ptr_origin = std::make_shared<FactorPixelHP>(feature_origin, lmk_hp, shared_from_this());
+ if (fac_ptr_origin != nullptr) // factor links
+ {
+ feature_origin->addFactor(fac_ptr_origin);
+ lmk->addConstrainedBy(fac_ptr_origin);
+ }
+ FactorPixelHPPtr fac_ptr_inc = std::make_shared<FactorPixelHP>(feature_inc, lmk_hp, shared_from_this());
+ if (fac_ptr_inc != nullptr) // factor links
+ {
+ feature_inc->addFactor(fac_ptr_inc);
+ lmk->addConstrainedBy(fac_ptr_inc);
+ }
+ }
+ else
+ {
+ LandmarkBasePtr lmk = lmk_track_map_[trkid];
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+
+ //Create factor
+ FactorPixelHPPtr fac_ptr_inc = std::make_shared<FactorPixelHP>(feature_inc, lmk_hp, shared_from_this());
+ if (fac_ptr_inc != nullptr) // factor links
+ {
+ feature_inc->addFactor(fac_ptr_inc);
+ lmk->addConstrainedBy(fac_ptr_inc);
+ }
+ }
+
+ }
+}
+
+
ProcessorBasePtr ProcessorBundleAdjustment::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
diff --git a/src/yaml/processor_bundle_adjustment_yaml.cpp b/src/yaml/processor_bundle_adjustment_yaml.cpp
index 5006e127e..ac6740fc5 100644
--- a/src/yaml/processor_bundle_adjustment_yaml.cpp
+++ b/src/yaml/processor_bundle_adjustment_yaml.cpp
@@ -1,10 +1,3 @@
-/*
- * processor_tracker_feature_trifocal_yaml.cpp
- *
- * Created on: Apr 12, 2018
- * Author: asantamaria
- */
-
// wolf yaml
#include "core/yaml/yaml_conversion.h"
@@ -46,19 +39,20 @@ static ProcessorParamsBasePtr createProcessorParamsBundleAdjustment(const std::s
YAML::Node algorithm = config ["algorithm"];
params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
params->voting_active = algorithm["voting active"] .as<bool>();
+ params->delete_ambiguities = algorithm["delete ambiguities"] .as<bool>();
params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
/*
params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
params->n_cells_v = algorithm["grid vert cells"] .as<int>();
params->min_response_new_feature = algorithm["min response new features"] .as<int>();
- params->min_track_length_for_constraint = algorithm["min track length for constraint"].as<int>();
*/
+ params->min_track_length_for_factor = algorithm["min track length for factor"].as<int>();
+
YAML::Node noise = config["noise"];
- /*
params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>();
- */
+
return params;
}
else
@@ -70,7 +64,7 @@ static ProcessorParamsBasePtr createProcessorParamsBundleAdjustment(const std::s
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_oriol = ProcessorParamsFactory::get().registerCreator("TRACKER BUNDLE ADJUSTMENT", createProcessorParamsBundleAdjustment);
+const bool WOLF_UNUSED registered_prc_bundle_adjustment = ProcessorParamsFactory::get().registerCreator("TRACKER BUNDLE ADJUSTMENT", createProcessorParamsBundleAdjustment);
} // namespace [unnamed]
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8c13de3f3..4868d66d2 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -18,4 +18,8 @@ target_link_libraries(gtest_sensor_camera ${PROJECT_NAME} ${wolf_LIBRARY} ${Open
# ProcessorFeatureTrifocal test
wolf_add_gtest(gtest_processor_tracker_feature_trifocal gtest_processor_tracker_feature_trifocal.cpp)
-target_link_libraries(gtest_processor_tracker_feature_trifocal ${PROJECT_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
\ No newline at end of file
+target_link_libraries(gtest_processor_tracker_feature_trifocal ${PROJECT_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
+
+# ProcessorBundleAdjustment test
+wolf_add_gtest(gtest_processor_bundle_adjustment gtest_processor_bundle_adjustment.cpp)
+target_link_libraries(gtest_processor_bundle_adjustment ${PROJECT_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
\ No newline at end of file
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
new file mode 100644
index 000000000..de12f3966
--- /dev/null
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -0,0 +1,80 @@
+
+#include "utils_gtest.h"
+
+#include "vision/sensor/sensor_camera.h"
+#include "core/sensor/sensor_factory.h"
+
+using namespace wolf;
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+//class ProcessorBundleAdjustment_class : public testing::Test{
+// public:
+// virtual void SetUp()
+// {
+// }
+//};
+
+TEST(ProcessorBundleAdjustment, installCamera)
+{
+ Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+ IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
+ params->width = 640;
+ params->height = 480;
+ params->pinhole_model_raw << 321, 241, 321, 321;
+ params->pinhole_model_rectified << 320, 240, 320, 320;
+ params->distortion = Eigen::Vector3s( 0, 0, 0 );
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 3D");
+
+ // Install camera
+ auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
+
+ ASSERT_NE(sen, nullptr);
+
+ SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+
+ ASSERT_NE(cam, nullptr);
+ ASSERT_EQ(cam->getImgWidth(), 640);
+}
+
+/*#include "vision/processor/processor_bundle_adjustment.h"
+#include "vision/capture/capture_image.h"
+
+#include "vision_utils.h"
+
+TEST(ProcessorBundleAdjustment, installProcessor)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 3D");
+
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+
+ // Install processor
+// ProcessorParamsBundleAdjustmentPtr params = make_shared<ProcessorParamsBundleAdjustment>();
+// params->delete_ambiguities = true;
+// params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+// params->pixel_noise_std = 1.0;
+// params->min_track_length_for_factor = 3;
+// params->voting_active = true;
+// params->max_new_features = 5;
+// auto proc = problem->installProcessor("BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+//
+// std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+//
+// ASSERT_EQ(proc->getProblem(), problem);
+// ASSERT_TRUE(problem->check());
+}*/
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
index 7c27882d3..ea70cb8c0 100644
--- a/test/gtest_sensor_camera.cpp
+++ b/test/gtest_sensor_camera.cpp
@@ -8,11 +8,35 @@
#include "utils_gtest.h"
-#include "src/sensor/sensor_camera.cpp"
+#include "vision/sensor/sensor_camera.h"
#include "core/sensor/sensor_factory.h"
using namespace wolf;
+TEST(SensorCamera, install)
+{
+ Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+ IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
+ params->width = 640;
+ params->height = 480;
+ params->pinhole_model_raw << 321, 241, 321, 321;
+ params->pinhole_model_rectified << 320, 240, 320, 320;
+ params->distortion = Eigen::Vector3s( 0, 0, 0 );
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 3D");
+
+ // Install camera
+ auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
+
+ ASSERT_NE(sen, nullptr);
+
+ SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+
+ ASSERT_NE(cam, nullptr);
+ ASSERT_EQ(cam->getImgWidth(), 640);
+}
+
TEST(SensorCamera, Img_size)
{
Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
--
GitLab
From e3774f053c203fa4b90d40820e4ca48a59cde0a2 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Tue, 14 May 2019 09:37:53 +0200
Subject: [PATCH 03/58] wip
---
test/gtest_processor_bundle_adjustment.cpp | 169 ++++++++++++++++-----
test/gtest_sensor_camera.cpp | 48 +++---
2 files changed, 151 insertions(+), 66 deletions(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 6fc7578c7..af538ada1 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -1,11 +1,54 @@
#include "utils_gtest.h"
+#include "vision_utils.h"
#include "vision/sensor/sensor_camera.h"
#include "core/sensor/sensor_factory.h"
+#include "vision/processor/processor_bundle_adjustment.h"
+#include "vision/capture/capture_image.h"
+
using namespace wolf;
+class Dummy : public ProcessorBundleAdjustment
+{
+ public:
+
+ Dummy(ProcessorParamsBundleAdjustmentPtr& _params_bundle_adjustment):ProcessorBundleAdjustment(_params_bundle_adjustment)
+ {
+ }
+
+ void setLast(CaptureImagePtr& _last_ptr)
+ {
+ last_ptr_ = _last_ptr;
+ capture_image_last_ = _last_ptr;
+ }
+ void setInc(CaptureImagePtr& _incoming_ptr)
+ {
+ incoming_ptr_ = _incoming_ptr;
+ capture_image_incoming_ = _incoming_ptr;
+ }
+
+ CaptureBasePtr getLast()
+ {
+ return last_ptr_;
+ }
+ CaptureBasePtr getInc()
+ {
+ return capture_image_incoming_;
+ }
+
+ void fillCaptureImage(CaptureImagePtr& im)
+ {
+ // Detect KeyPoints
+ im->keypoints_ = det_ptr_->detect(im->getImage());
+ // Compute Descriptors
+ im->descriptors_ = des_ptr_->getDescriptor(im->getImage(), im->keypoints_);
+
+ }
+};
+
+
// Use the following in case you want to initialize tests with predefines variables or methods.
//class ProcessorBundleAdjustment_class : public testing::Test{
// public:
@@ -14,63 +57,105 @@ using namespace wolf;
// }
//};
-TEST(ProcessorBundleAdjustment, installCamera)
+TEST(ProcessorBundleAdjustment, installProcessor)
{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
- params->width = 640;
- params->height = 480;
- params->pinhole_model_raw << 321, 241, 321, 321;
- params->pinhole_model_rectified << 320, 240, 320, 320;
- params->distortion = Eigen::Vector3s( 0, 0, 0 );
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::cout << wolf_root << std::endl;
// Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
+ ProblemPtr problem = Problem::create("PO", 3);
- // Install camera
- auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
- ASSERT_NE(sen, nullptr);
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem->installProcessor("BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+ ASSERT_EQ(proc->getProblem(), problem);
+ ASSERT_TRUE(problem->check());
+}
- SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+TEST(ProcessorBundleAdjustment, preProcess)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
- ASSERT_NE(cam, nullptr);
- ASSERT_EQ(cam->getImgWidth(), 640);
-}
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = std::make_shared<ProcessorBundleAdjustment>(params);
-/*#include "vision/processor/processor_bundle_adjustment.h"
-#include "vision/capture/capture_image.h"
+ // Fill incoming_ptr with an image
+ Dummy init = Dummy(params);
+ const Scalar time = 0.0;
+ TimeStamp ts(time);
+ CaptureImagePtr im = std::make_shared<CaptureImage>(ts, nullptr, cv::imread(wolf_root + "/src/examples/Test_gazebo_x-20cm_y-20cm.jpg"));
+ init.setInc(im);
+ proc->preProcess();
-#include "vision_utils.h"
+ ASSERT_NE(init.getLast(), nullptr);
-TEST(ProcessorBundleAdjustment, installProcessor)
+
+ // Test detectNewFeatures
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+ unsigned int num = proc->detectNewFeatures(params->max_new_features, feat_list);
+
+ ASSERT_EQ(num, params->max_new_features);
+}
+
+TEST(ProcessorBundleAdjustment, detectNewFeatures)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = std::make_shared<ProcessorBundleAdjustment>(params);
+
+ // Fill last_ptr with an image
+ Dummy init = Dummy(params);
+ const Scalar time = 0.0;
+ TimeStamp ts(time);
+ CaptureImagePtr im = std::make_shared<CaptureImage>(ts, nullptr, cv::imread(wolf_root + "/src/examples/Test_gazebo_x-20cm_y-20cm.jpg"));
+ init.setLast(im);
+ std::cout << "Set last_ptr_" << std::endl; //TODO: Seg fault
+ init.fillCaptureImage(im);
+
+ CaptureImagePtr cap = std::static_pointer_cast<CaptureImage>(init.getInc());
+ ASSERT_NE(cap->keypoints_.size(), 0);
+
+
+ // Test detectNewFeatures
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+ unsigned int num = proc->detectNewFeatures(params->max_new_features, feat_list);
+
+ ASSERT_EQ(num, params->max_new_features);
+}
+
+
- // Install camera
- IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
- intr->width = 640;
- intr->height = 480;
- auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
- // Install processor
-// ProcessorParamsBundleAdjustmentPtr params = make_shared<ProcessorParamsBundleAdjustment>();
-// params->delete_ambiguities = true;
-// params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
-// params->pixel_noise_std = 1.0;
-// params->min_track_length_for_factor = 3;
-// params->voting_active = true;
-// params->max_new_features = 5;
-// auto proc = problem->installProcessor("BUNDLE ADJUSTMENT", "processor", sens_cam, params);
-//
-// std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-//
-// ASSERT_EQ(proc->getProblem(), problem);
-// ASSERT_TRUE(problem->check());
-}*/
int main(int argc, char **argv)
{
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
index c8fefadf6..a3810afc0 100644
--- a/test/gtest_sensor_camera.cpp
+++ b/test/gtest_sensor_camera.cpp
@@ -13,30 +13,6 @@
using namespace wolf;
-TEST(SensorCamera, install)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
- params->width = 640;
- params->height = 480;
- params->pinhole_model_raw << 321, 241, 321, 321;
- params->pinhole_model_rectified << 320, 240, 320, 320;
- params->distortion = Eigen::Vector3s( 0, 0, 0 );
-
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
-
- // Install camera
- auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
-
- ASSERT_NE(sen, nullptr);
-
- SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
-
- ASSERT_NE(cam, nullptr);
- ASSERT_EQ(cam->getImgWidth(), 640);
-}
-
TEST(SensorCamera, Img_size)
{
Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
@@ -139,6 +115,30 @@ TEST(SensorCamera, create)
ASSERT_EQ(cam->getImgWidth(), 640);
}
+TEST(SensorCamera, install)
+{
+ Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+ IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
+ params->width = 640;
+ params->height = 480;
+ params->pinhole_model_raw << 321, 241, 321, 321;
+ params->pinhole_model_rectified << 320, 240, 320, 320;
+ params->distortion = Eigen::Vector3s( 0, 0, 0 );
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 3D",3);
+
+ // Install camera
+ auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
+
+ ASSERT_NE(sen, nullptr);
+
+ SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+
+ ASSERT_NE(cam, nullptr);
+ ASSERT_EQ(cam->getImgWidth(), 640);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
--
GitLab
From f0ab3f7aa4fab849110e7d3bd0a4c2655090cef4 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Tue, 14 May 2019 11:58:09 +0200
Subject: [PATCH 04/58] wip gtest
---
src/processor/processor_bundle_adjustment.cpp | 8 +-
test/gtest_processor_bundle_adjustment.cpp | 118 +++++++++---------
2 files changed, 62 insertions(+), 64 deletions(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index c39ee19e4..16215b6d4 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -72,10 +72,10 @@ void ProcessorBundleAdjustment::preProcess()
{
std::map<int,int> ambiguities_map;
for (auto match : capture_image_incoming_->matches_from_precedent_)
- if (ambiguities_map.count(match.trainIdx))
- ambiguities_map[match.trainIdx] = 0;
- else
- ambiguities_map[match.trainIdx] = 1;
+ if (ambiguities_map.count(match.trainIdx)) //if ambiguity
+ ambiguities_map[match.trainIdx] = 1; //ambiguity true
+ else //if not ambiguity
+ ambiguities_map[match.trainIdx] = 0; //ambiguity false
// Set capture map of match indices
for (auto match : capture_image_incoming_->matches_from_precedent_)
{
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index af538ada1..126c76d39 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -10,45 +10,40 @@
using namespace wolf;
-class Dummy : public ProcessorBundleAdjustment
+std::string wolf_root = "/home/ovendrell/dev/vision";
+
+class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{
public:
- Dummy(ProcessorParamsBundleAdjustmentPtr& _params_bundle_adjustment):ProcessorBundleAdjustment(_params_bundle_adjustment)
- {
- }
+ ProcessorBundleAdjustmentDummy(ProcessorParamsBundleAdjustmentPtr& _params_bundle_adjustment):ProcessorBundleAdjustment(_params_bundle_adjustment){}
- void setLast(CaptureImagePtr& _last_ptr)
+ void setLast(CaptureImagePtr _last_ptr)
{
last_ptr_ = _last_ptr;
capture_image_last_ = _last_ptr;
}
- void setInc(CaptureImagePtr& _incoming_ptr)
+ void setInc(CaptureImagePtr _incoming_ptr)
{
incoming_ptr_ = _incoming_ptr;
capture_image_incoming_ = _incoming_ptr;
}
- CaptureBasePtr getLast()
- {
- return last_ptr_;
- }
- CaptureBasePtr getInc()
- {
- return capture_image_incoming_;
- }
-
- void fillCaptureImage(CaptureImagePtr& im)
+ CaptureImagePtr createCaptureImage(std::string _path, bool detectAndDescript = false)
{
- // Detect KeyPoints
- im->keypoints_ = det_ptr_->detect(im->getImage());
- // Compute Descriptors
- im->descriptors_ = des_ptr_->getDescriptor(im->getImage(), im->keypoints_);
-
+ const Scalar time = 0.0;
+ TimeStamp ts(time);
+ CaptureImagePtr im = std::make_shared<CaptureImage>(ts, nullptr, cv::imread(_path));
+ if (detectAndDescript){
+ // Detect KeyPoints
+ im->keypoints_ = det_ptr_->detect(im->getImage());
+ // Compute Descriptors
+ im->descriptors_ = des_ptr_->getDescriptor(im->getImage(), im->keypoints_);
+ }
+ return im;
}
};
-
// Use the following in case you want to initialize tests with predefines variables or methods.
//class ProcessorBundleAdjustment_class : public testing::Test{
// public:
@@ -59,8 +54,8 @@ class Dummy : public ProcessorBundleAdjustment
TEST(ProcessorBundleAdjustment, installProcessor)
{
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::cout << wolf_root << std::endl;
+// std::string wolf_root = _WOLF_ROOT_DIR;
+// std::cout << wolf_root << std::endl;
// Wolf problem
ProblemPtr problem = Problem::create("PO", 3);
@@ -79,7 +74,7 @@ TEST(ProcessorBundleAdjustment, installProcessor)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
- auto proc = problem->installProcessor("BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
@@ -89,7 +84,7 @@ TEST(ProcessorBundleAdjustment, installProcessor)
TEST(ProcessorBundleAdjustment, preProcess)
{
- std::string wolf_root = _WOLF_ROOT_DIR;
+// std::string wolf_root = _WOLF_ROOT_DIR;
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
@@ -99,29 +94,26 @@ TEST(ProcessorBundleAdjustment, preProcess)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
- auto proc = std::make_shared<ProcessorBundleAdjustment>(params);
-
- // Fill incoming_ptr with an image
- Dummy init = Dummy(params);
- const Scalar time = 0.0;
- TimeStamp ts(time);
- CaptureImagePtr im = std::make_shared<CaptureImage>(ts, nullptr, cv::imread(wolf_root + "/src/examples/Test_gazebo_x-20cm_y-20cm.jpg"));
- init.setInc(im);
- proc->preProcess();
-
- ASSERT_NE(init.getLast(), nullptr);
-
- // Test detectNewFeatures
- FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- unsigned int num = proc->detectNewFeatures(params->max_new_features, feat_list);
-
- ASSERT_EQ(num, params->max_new_features);
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
+
+ // Put an image on incoming_ptr_
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg");
+ proc_dummy->setInc(image_inc_ptr);
+ // Put an image on last_ptr_
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg", true);
+ proc_dummy->setLast(image_last_ptr);
+ // Test dpreProcess
+ proc_dummy->preProcess();
+ CaptureImagePtr inc = std::static_pointer_cast<CaptureImage>(proc_dummy->getIncoming());
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ ASSERT_EQ(inc->keypoints_.size(), last->keypoints_.size());
+ ASSERT_NE(inc->keypoints_.size(), 0);
}
TEST(ProcessorBundleAdjustment, detectNewFeatures)
{
- std::string wolf_root = _WOLF_ROOT_DIR;
+// std::string wolf_root = _WOLF_ROOT_DIR;
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
@@ -131,30 +123,36 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
- auto proc = std::make_shared<ProcessorBundleAdjustment>(params);
-
- // Fill last_ptr with an image
- Dummy init = Dummy(params);
- const Scalar time = 0.0;
- TimeStamp ts(time);
- CaptureImagePtr im = std::make_shared<CaptureImage>(ts, nullptr, cv::imread(wolf_root + "/src/examples/Test_gazebo_x-20cm_y-20cm.jpg"));
- init.setLast(im);
- std::cout << "Set last_ptr_" << std::endl; //TODO: Seg fault
- init.fillCaptureImage(im);
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
- CaptureImagePtr cap = std::static_pointer_cast<CaptureImage>(init.getInc());
- ASSERT_NE(cap->keypoints_.size(), 0);
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+ // Put an image on last_ptr_
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg", true);
+ ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
+ proc_dummy->setLast(image_last_ptr);
// Test detectNewFeatures
- FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- unsigned int num = proc->detectNewFeatures(params->max_new_features, feat_list);
+ unsigned int num = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ ASSERT_EQ(num, 0);
- ASSERT_EQ(num, params->max_new_features);
-}
+ // Put an image on incoming_ptr_
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg");
+ proc_dummy->setInc(image_inc_ptr);
+ // Test detectNewFeatures
+ unsigned int num2 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ ASSERT_EQ(num2, 0);
+ // preProcess Incoming to fill last_ptr_ map_index_to_next_
+ proc_dummy->preProcess();
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ ASSERT_NE(last->map_index_to_next_.size(),0);
+ // Test detectNewFeatures
+ unsigned int num3 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ ASSERT_EQ(num3, params->max_new_features);
+}
int main(int argc, char **argv)
--
GitLab
From 7869a7137d50b4ca7345e71b4db2b1fde5f4b380 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Tue, 14 May 2019 12:57:36 +0200
Subject: [PATCH 05/58] WIP
---
src/processor/processor_bundle_adjustment.cpp | 4 +-
test/gtest_processor_bundle_adjustment.cpp | 136 +++++++++++++++++-
2 files changed, 131 insertions(+), 9 deletions(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 16215b6d4..c2d82494b 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -45,7 +45,7 @@ ProcessorBundleAdjustment::ProcessorBundleAdjustment(ProcessorParamsBundleAdjust
void ProcessorBundleAdjustment::configure(SensorBasePtr _sensor)
{
- //TODO: Implement
+ //TODO: Implement if needed
}
void ProcessorBundleAdjustment::preProcess()
@@ -95,7 +95,7 @@ void ProcessorBundleAdjustment::preProcess()
void ProcessorBundleAdjustment::postProcess()
{
- //TODO: visualization
+ //TODO: check visualization
std::map<int,std::list<vision_utils::KeyPointEnhanced> > kp_enh_tracks;
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 126c76d39..09766bf6f 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -6,11 +6,13 @@
#include "core/sensor/sensor_factory.h"
#include "vision/processor/processor_bundle_adjustment.h"
#include "vision/capture/capture_image.h"
+#include "vision/internal/config.h"
using namespace wolf;
std::string wolf_root = "/home/ovendrell/dev/vision";
+//std::string wolf_root = _WOLF_VISION_ROOT_DIR;
class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{
@@ -69,7 +71,7 @@ TEST(ProcessorBundleAdjustment, installProcessor)
// Install processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -89,7 +91,7 @@ TEST(ProcessorBundleAdjustment, preProcess)
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -98,10 +100,10 @@ TEST(ProcessorBundleAdjustment, preProcess)
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
proc_dummy->setInc(image_inc_ptr);
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
proc_dummy->setLast(image_last_ptr);
// Test dpreProcess
proc_dummy->preProcess();
@@ -118,7 +120,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/src/examples/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -128,7 +130,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
proc_dummy->setLast(image_last_ptr);
@@ -137,7 +139,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/src/examples/Test_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
proc_dummy->setInc(image_inc_ptr);
// Test detectNewFeatures
@@ -154,6 +156,126 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_EQ(num3, params->max_new_features);
}
+TEST(ProcessorBundleAdjustment, trackFeatures)
+{
+// std::string wolf_root = _WOLF_ROOT_DIR;
+
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
+
+ //fill feat_last list
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
+ proc_dummy->setLast(image_last_ptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
+ proc_dummy->setInc(image_inc_ptr);
+ proc_dummy->preProcess();
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ //Test trackFeatures
+ FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
+ FeatureMatchMap feat_correspondance = FeatureMatchMap();
+ proc_dummy->trackFeatures(feat_list, feat_list_out, feat_correspondance);
+ ASSERT_EQ(feat_list.size(), feat_list_out.size());
+}
+
+TEST(ProcessorBundleAdjustment, postProcess)
+{
+// std::string wolf_root = _WOLF_ROOT_DIR;
+
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
+
+ //fill feat_last list
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
+ proc_dummy->setLast(image_last_ptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
+ proc_dummy->setInc(image_inc_ptr);
+ proc_dummy->preProcess();
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ //Test trackFeatures
+ FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
+ FeatureMatchMap feat_correspondance = FeatureMatchMap();
+ proc_dummy->trackFeatures(feat_list, feat_list_out, feat_correspondance);
+
+ proc_dummy->postProcess();
+
+}
+/*
+TEST(ProcessorBundleAdjustment,fullTest)
+{
+ // std::string wolf_root = _WOLF_ROOT_DIR;
+ // std::cout << wolf_root << std::endl;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+
+ // initialize
+ Scalar dt = 0.01
+ TimeStamp t(0.0);
+ Vector7s x; x << 0,0,0, 0,0,0,1;
+ Matrix6s P = Matrix6s::Identity() * 0.000001;
+ problem->setPrior(x, P, t, dt/2); // KF1
+
+ // Track
+ cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols)
+ image *= 0; // TODO see if we want to use a real image
+ SensorCameraPtr sens_trk_cam = std::static_pointer_cast<SensorCamera>(sens_cam);
+ CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
+ proc->process(capt_trk);
+
+ problem->print(2,0,1,0);
+
+ for (size_t ii=0; ii<8; ii++ )
+ {
+ // Move
+ t = t+dt;
+ WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+
+ // Track
+ capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
+ proc_trk->process(capt_trk);
+
+ problem->print(2,0,1,0);
+
+ // Only odom creating KFs
+ ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 3D")==0 );
+ }
+}
+}*/
+
int main(int argc, char **argv)
{
--
GitLab
From 93cf66e8ded58be23a4b42934d2bec212adfe3dc Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Tue, 14 May 2019 13:04:21 +0200
Subject: [PATCH 06/58] WIP
---
test/gtest_processor_bundle_adjustment.cpp | 32 +++++++++++-----------
1 file changed, 16 insertions(+), 16 deletions(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 09766bf6f..fc24298c7 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -47,7 +47,7 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
};
// Use the following in case you want to initialize tests with predefines variables or methods.
-//class ProcessorBundleAdjustment_class : public testing::Test{
+//class ProcessorBundleAdjustment_class : public testing::demo{
// public:
// virtual void SetUp()
// {
@@ -100,12 +100,12 @@ TEST(ProcessorBundleAdjustment, preProcess)
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
proc_dummy->setInc(image_inc_ptr);
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
proc_dummy->setLast(image_last_ptr);
- // Test dpreProcess
+ // demo dpreProcess
proc_dummy->preProcess();
CaptureImagePtr inc = std::static_pointer_cast<CaptureImage>(proc_dummy->getIncoming());
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
@@ -130,19 +130,19 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
proc_dummy->setLast(image_last_ptr);
- // Test detectNewFeatures
+ // demo detectNewFeatures
unsigned int num = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
proc_dummy->setInc(image_inc_ptr);
- // Test detectNewFeatures
+ // demo detectNewFeatures
unsigned int num2 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
ASSERT_EQ(num2, 0);
@@ -151,7 +151,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
ASSERT_NE(last->map_index_to_next_.size(),0);
- // Test detectNewFeatures
+ // demo detectNewFeatures
unsigned int num3 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
ASSERT_EQ(num3, params->max_new_features);
}
@@ -172,14 +172,14 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
//fill feat_last list
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
proc_dummy->setLast(image_last_ptr);
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
- //Test trackFeatures
+ //demo trackFeatures
FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
FeatureMatchMap feat_correspondance = FeatureMatchMap();
proc_dummy->trackFeatures(feat_list, feat_list_out, feat_correspondance);
@@ -202,14 +202,14 @@ TEST(ProcessorBundleAdjustment, postProcess)
//fill feat_last list
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
proc_dummy->setLast(image_last_ptr);
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/Test_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
- //Test trackFeatures
+ //demo trackFeatures
FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
FeatureMatchMap feat_correspondance = FeatureMatchMap();
proc_dummy->trackFeatures(feat_list, feat_list_out, feat_correspondance);
@@ -218,7 +218,7 @@ TEST(ProcessorBundleAdjustment, postProcess)
}
/*
-TEST(ProcessorBundleAdjustment,fullTest)
+TEST(ProcessorBundleAdjustment,fulldemo)
{
// std::string wolf_root = _WOLF_ROOT_DIR;
// std::cout << wolf_root << std::endl;
--
GitLab
From fbd986435cc3fd1702b6ea2637451de3acca691b Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Tue, 14 May 2019 14:19:30 +0200
Subject: [PATCH 07/58] Added config file for vision plugin
---
internal/config.h.in | 4 ++--
test/gtest_processor_bundle_adjustment.cpp | 4 ++--
2 files changed, 4 insertions(+), 4 deletions(-)
diff --git a/internal/config.h.in b/internal/config.h.in
index 9f12d5e4a..b9008e196 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,8 +24,8 @@
// which will be added to the include path for compilation,
// and installed with the public wolf headers.
-#ifndef WOLF_INTERNAL_CONFIG_H_
-#define WOLF_INTERNAL_CONFIG_H_
+#ifndef WOLF_INTERNAL_${UPPERNAME}_CONFIG_H_
+#define WOLF_INTERNAL_${UPPERNAME}_CONFIG_H_
#cmakedefine _WOLF_DEBUG
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index fc24298c7..f1906e0e4 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -11,8 +11,8 @@
using namespace wolf;
-std::string wolf_root = "/home/ovendrell/dev/vision";
-//std::string wolf_root = _WOLF_VISION_ROOT_DIR;
+// std::string wolf_root = "/home/ovendrell/dev/vision";
+std::string wolf_root = _WOLF_VISION_ROOT_DIR;
class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{
--
GitLab
From 2f11f97d9f70fa6dce3971e15728137954bfd5fc Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Thu, 16 May 2019 13:34:17 +0200
Subject: [PATCH 08/58] wip
---
src/processor/processor_bundle_adjustment.cpp | 81 ++++--
test/gtest_processor_bundle_adjustment.cpp | 272 +++++++++++-------
2 files changed, 228 insertions(+), 125 deletions(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index c2d82494b..e2daf4031 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -70,17 +70,17 @@ void ProcessorBundleAdjustment::preProcess()
//TODO: Get only the best ones
if (params_bundle_adjustment_->delete_ambiguities) //filter ambiguities
{
- std::map<int,int> ambiguities_map;
+ std::map<int,bool> ambiguities_map;
for (auto match : capture_image_incoming_->matches_from_precedent_)
if (ambiguities_map.count(match.trainIdx)) //if ambiguity
- ambiguities_map[match.trainIdx] = 1; //ambiguity true
+ ambiguities_map[match.trainIdx] = true; //ambiguity true
else //if not ambiguity
- ambiguities_map[match.trainIdx] = 0; //ambiguity false
+ ambiguities_map[match.trainIdx] = false; //ambiguity false
// Set capture map of match indices
for (auto match : capture_image_incoming_->matches_from_precedent_)
{
if (!ambiguities_map.at(match.trainIdx))
- capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
+ capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
}
}
else
@@ -135,11 +135,11 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
{
FeaturePointImagePtr feat_last = std::static_pointer_cast<FeaturePointImage>(feat_base);
- if (capture_image_last_->map_index_to_next_.count(feat_last->getIndexKeyPoint())) //If the track exists
+ if (capture_image_last_->map_index_to_next_.count(feat_last->getIndexKeyPoint())) //If the track to incoming exists
{
int index_inc = capture_image_last_->map_index_to_next_.at(feat_last->getIndexKeyPoint());
- if (capture_image_incoming_->matches_normalized_scores_.at(index_inc) > mat_ptr_->getParams()->min_norm_score )
+ if (true)//capture_image_incoming_->matches_normalized_scores_.at(index_inc) > mat_ptr_->getParams()->min_norm_score )
{
// Get kp incoming and keypoint last
cv::KeyPoint kp_inc = capture_image_incoming_->keypoints_.at(index_inc);
@@ -153,13 +153,12 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
auto feature_match_ptr = std::make_shared<FeatureMatch>();
feature_match_ptr->feature_ptr_= feat_last;
- feature_match_ptr->normalized_score_ = capture_image_incoming_->matches_normalized_scores_.at(index_inc);
+ feature_match_ptr->normalized_score_ = 1.0;//capture_image_incoming_->matches_normalized_scores_.at(index_inc);
_feature_correspondences[feat_inc] = feature_match_ptr;
-
}
}
}
-
+// std::cout << "RUNNING trackFeatures()\n";
return _feature_correspondences.size();
}
@@ -201,6 +200,9 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
}
}
+ std::cout << "RUNNING detectNewFeatures()\n";
+
+
return _features_last_out.size();
}
@@ -209,7 +211,11 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
bool ProcessorBundleAdjustment::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
{
//TODO: Implement
- return false;
+//// assert(_origin_feature!=nullptr);
+// assert( _last_feature!=nullptr);
+// assert(_incoming_feature!=nullptr);
+// std::cout << "RUNNING correctFeatureDrift()\n";
+ return true;
}
bool ProcessorBundleAdjustment::voteForKeyFrame()
@@ -253,7 +259,6 @@ FactorBasePtr ProcessorBundleAdjustment::createFactor(FeatureBasePtr _feature_pt
LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _feature_ptr)
{
-
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
//FrameBasePtr anchor_frame = getLast()->getFrame();
@@ -262,6 +267,7 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
//Scalar distance = params_bundle_adjustment_->distance; // arbitrary value
+ Scalar distance = 1;
Eigen::Vector4s vec_homogeneous;
point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
@@ -273,9 +279,22 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
point3D.normalize();
- vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/1};//distance};
+ vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
- LandmarkHPPtr lmk_hp_ptr = std::make_shared<LandmarkHP>(vec_homogeneous, getSensor(), feat_point_image_ptr->getDescriptor());
+
+ //TODO: lmk from camera to world coordinate frame.
+
+ Transform<Scalar,3,Affine> T_w_r
+ = Translation<Scalar,3>(_feature_ptr->getFrame()->getP()->getState())
+ * Quaternions(_feature_ptr->getFrame()->getO()->getState().data());
+ Transform<Scalar,3,Affine> T_r_c
+ = Translation<Scalar,3>(_feature_ptr->getCapture()->getSensorP()->getState())
+ * Quaternions(_feature_ptr->getCapture()->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> vec_homogeneous_w = T_w_r
+ * T_r_c
+ * vec_homogeneous;
+
+ LandmarkHPPtr lmk_hp_ptr = std::make_shared<LandmarkHP>(vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
_feature_ptr->setLandmarkId(lmk_hp_ptr->id());
return lmk_hp_ptr;
}
@@ -283,35 +302,47 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
void ProcessorBundleAdjustment::establishFactors()
{
- TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, incoming_ptr_);
+ TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, last_ptr_);
for (auto const & pair_trkid_pair : matches_origin_inc)
{
size_t trkid = pair_trkid_pair.first;
FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
- FeatureBasePtr feature_inc = pair_trkid_pair.second.second;
+ FeatureBasePtr feature_last = pair_trkid_pair.second.second;
+
+ assert(feature_origin!=nullptr && "null feature origin");
+ assert(feature_last!=nullptr && "null feature last");
+ assert(feature_origin->getCapture()!=nullptr);
+ assert(feature_last->getCapture()!=nullptr);
+ assert(feature_last->getCapture()->getFrame()!=nullptr && feature_last->getCapture()->getFrame()->isKey());
+ assert(feature_origin->getCapture()->getFrame()!=nullptr && feature_origin->getCapture()->getFrame()->isKey());
- //TODO: don't use map[track_id] = landmarkptr.
if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
{
//createLandmark
LandmarkBasePtr lmk = createLandmark(feature_origin);
+ assert(lmk!=nullptr);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+ getProblem()->addLandmark(lmk_hp);
//add Landmark to map: map[track_id] = landmarkptr
lmk_track_map_[trkid] = lmk;
//create factors
+// assert(feature_origin->getCapture()->getSensor() != nullptr);
+// assert(feature_origin->getCapture()->getSensorP() != nullptr);
+// assert(feature_origin->getCapture()->getSensorO() != nullptr);
+// assert(lmk->getP() != nullptr);
FactorPixelHPPtr fac_ptr_origin = std::make_shared<FactorPixelHP>(feature_origin, lmk_hp, shared_from_this());
if (fac_ptr_origin != nullptr) // factor links
{
feature_origin->addFactor(fac_ptr_origin);
lmk->addConstrainedBy(fac_ptr_origin);
}
- FactorPixelHPPtr fac_ptr_inc = std::make_shared<FactorPixelHP>(feature_inc, lmk_hp, shared_from_this());
- if (fac_ptr_inc != nullptr) // factor links
+ FactorPixelHPPtr fac_ptr_last = std::make_shared<FactorPixelHP>(feature_last, lmk_hp, shared_from_this());
+ if (fac_ptr_last != nullptr) // factor links
{
- feature_inc->addFactor(fac_ptr_inc);
- lmk->addConstrainedBy(fac_ptr_inc);
+ feature_last->addFactor(fac_ptr_last);
+ lmk->addConstrainedBy(fac_ptr_last);
}
}
else
@@ -320,19 +351,17 @@ void ProcessorBundleAdjustment::establishFactors()
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
//Create factor
- FactorPixelHPPtr fac_ptr_inc = std::make_shared<FactorPixelHP>(feature_inc, lmk_hp, shared_from_this());
- if (fac_ptr_inc != nullptr) // factor links
+ FactorPixelHPPtr fac_ptr_last = std::make_shared<FactorPixelHP>(feature_last, lmk_hp, shared_from_this());
+ if (fac_ptr_last != nullptr) // factor links
{
- feature_inc->addFactor(fac_ptr_inc);
- lmk->addConstrainedBy(fac_ptr_inc);
+ feature_last->addFactor(fac_ptr_last);
+ lmk->addConstrainedBy(fac_ptr_last);
}
}
}
}
-
-
ProcessorBasePtr ProcessorBundleAdjustment::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index fc24298c7..c87bc3d91 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -8,11 +8,16 @@
#include "vision/capture/capture_image.h"
#include "vision/internal/config.h"
+// Vision utils includes
+#include <vision_utils.h>
+#include <sensors.h>
+#include <common_class/buffer.h>
+#include <common_class/frame.h>
using namespace wolf;
-std::string wolf_root = "/home/ovendrell/dev/vision";
-//std::string wolf_root = _WOLF_VISION_ROOT_DIR;
+//std::string wolf_root = "/home/ovendrell/dev/vision";
+std::string wolf_root = _WOLF_VISION_ROOT_DIR;
class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{
@@ -31,11 +36,40 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
capture_image_incoming_ = _incoming_ptr;
}
- CaptureImagePtr createCaptureImage(std::string _path, bool detectAndDescript = false)
+ std::map<size_t, LandmarkBasePtr> getLandmarkMap()
+ {
+ return lmk_track_map_;
+ }
+
+ unsigned int getProcessKnown()
+ {
+ return this->processKnown();
+ }
+
+ unsigned int getProcessNew(const int& _max_new_features)
+ {
+ return this->processNew(_max_new_features);
+ }
+
+ TrackMatrix getTrackMatrix()
+ {
+ return track_matrix_;
+ }
+
+ FeatureBasePtrList getKnownFeaturesIncoming()
+ {
+ return known_features_incoming_;
+ }
+ FeatureMatchMap getMatchesLastFromIncoming()
+ {
+ return matches_last_from_incoming_;
+ }
+
+ CaptureImagePtr createCaptureImage(std::string _path, SensorCameraPtr _sensor, bool detectAndDescript = false)
{
const Scalar time = 0.0;
TimeStamp ts(time);
- CaptureImagePtr im = std::make_shared<CaptureImage>(ts, nullptr, cv::imread(_path));
+ CaptureImagePtr im = std::make_shared<CaptureImage>(ts, _sensor, cv::imread(_path));
if (detectAndDescript){
// Detect KeyPoints
im->keypoints_ = det_ptr_->detect(im->getImage());
@@ -56,9 +90,6 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
TEST(ProcessorBundleAdjustment, installProcessor)
{
-// std::string wolf_root = _WOLF_ROOT_DIR;
-// std::cout << wolf_root << std::endl;
-
// Wolf problem
ProblemPtr problem = Problem::create("PO", 3);
@@ -86,8 +117,6 @@ TEST(ProcessorBundleAdjustment, installProcessor)
TEST(ProcessorBundleAdjustment, preProcess)
{
-// std::string wolf_root = _WOLF_ROOT_DIR;
-
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
@@ -100,10 +129,10 @@ TEST(ProcessorBundleAdjustment, preProcess)
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr);
// demo dpreProcess
proc_dummy->preProcess();
@@ -115,8 +144,6 @@ TEST(ProcessorBundleAdjustment, preProcess)
TEST(ProcessorBundleAdjustment, detectNewFeatures)
{
-// std::string wolf_root = _WOLF_ROOT_DIR;
-
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
@@ -130,7 +157,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
proc_dummy->setLast(image_last_ptr);
@@ -139,7 +166,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
// demo detectNewFeatures
@@ -158,8 +185,6 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
TEST(ProcessorBundleAdjustment, trackFeatures)
{
-// std::string wolf_root = _WOLF_ROOT_DIR;
-
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
@@ -172,9 +197,9 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
//fill feat_last list
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr);
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
@@ -186,95 +211,144 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
ASSERT_EQ(feat_list.size(), feat_list_out.size());
}
-TEST(ProcessorBundleAdjustment, postProcess)
+
+TEST(ProcessorBundleAdjustment, establishFactors)
{
-// std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Create processor
- ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
- params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
- params->pixel_noise_std = 1.0;
- params->min_track_length_for_factor = 3;
- params->voting_active = true;
- params->max_new_features = 5;
- auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
-
- //fill feat_last list
- FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", true);
- proc_dummy->setLast(image_last_ptr);
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg");
- proc_dummy->setInc(image_inc_ptr);
- proc_dummy->preProcess();
- CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
- proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
- //demo trackFeatures
- FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
- FeatureMatchMap feat_correspondance = FeatureMatchMap();
- proc_dummy->trackFeatures(feat_list, feat_list_out, feat_correspondance);
-
- proc_dummy->postProcess();
+ std::cout << "EMPTY Test\n";
+}
+TEST(ProcessorBundleAdjustment, createLandmark)
+{
+ std::cout << "EMPTY Test\n";
}
-/*
-TEST(ProcessorBundleAdjustment,fulldemo)
+
+
+TEST(ProcessorBundleAdjustment, process)
{
- // std::string wolf_root = _WOLF_ROOT_DIR;
- // std::cout << wolf_root << std::endl;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
-
- // Install camera
- IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
- intr->width = 640;
- intr->height = 480;
- auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
-
- // Install processor
- ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
- params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
- params->pixel_noise_std = 1.0;
- params->min_track_length_for_factor = 3;
- params->voting_active = true;
- params->max_new_features = 5;
- auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
-
- // initialize
- Scalar dt = 0.01
- TimeStamp t(0.0);
- Vector7s x; x << 0,0,0, 0,0,0,1;
- Matrix6s P = Matrix6s::Identity() * 0.000001;
- problem->setPrior(x, P, t, dt/2); // KF1
-
- // Track
- cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols)
- image *= 0; // TODO see if we want to use a real image
- SensorCameraPtr sens_trk_cam = std::static_pointer_cast<SensorCamera>(sens_cam);
- CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
- proc->process(capt_trk);
-
- problem->print(2,0,1,0);
-
- for (size_t ii=0; ii<8; ii++ )
- {
- // Move
- t = t+dt;
- WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
- // Track
- capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
- proc_trk->process(capt_trk);
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->width = 640;
+ intr->height = 480;
+ SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
- problem->print(2,0,1,0);
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
+
+ //1ST TIME
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ proc_dummy->process(image_inc_ptr);
+ ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
+
+ //2ND TIME
+ CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+// proc_dummy->setInc(image_inc_ptr2);
+// proc_dummy->preProcess();
+// proc_dummy->getProcessKnown();
+// proc_dummy->getProcessNew(params->max_new_features);
+// proc_dummy->establishFactors();
+ proc_dummy->process(image_inc_ptr2);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), params->max_new_features);
+ ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+
+ //3RD TIME -- RUNNING
+ CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ assert(proc_dummy->getOrigin()!=nullptr);
+ assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
+ proc_dummy->process(image_inc_ptr3);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), params->max_new_features);
+ ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+
+ //4TH TIME -- RUNNING
+ CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ proc_dummy->process(image_inc_ptr4);
+ ASSERT_EQ(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
+}
+
+TEST(ProcessorBundleAdjustment, processVideo)
+{
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->width = 640;
+ intr->height = 480;
+ SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
+
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = false;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 400;
+ params->min_features_for_keyframe = 0;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
+
+ //==================================vision_utils ============================================
+ vision_utils::SensorCameraPtr sen_ptr = std::make_shared<vision_utils::SensorCamera>();
+ sen_ptr->open("/home/ovendrell/eclipse-workspace/ddm_triad/data/video/VID4b.mp4");
+ //"/home/ovendrell/dev/vision_utils/src/test/data/test_usb_cam.mp4");
+
+ unsigned int buffer_size = 10;
+ vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
+ frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
+
+ unsigned int img_width = frame_buff.back()->getImage().cols;
+ unsigned int img_height = frame_buff.back()->getImage().rows;
+ std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
+ //==================================vision_utils ============================================
+
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ camera->setImgWidth(img_width);
+ camera->setImgHeight(img_height);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ TimeStamp t = 0;
+ // main loop
+ Scalar dt = 0.04;
+ for(int frame_count = 0; frame_count<150; ++frame_count)
+ {
+ t += dt;
+ std::cout << "Avis 0" << std::endl;
+ // Image ---------------------------------------------
+ frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
+ std::cout << "Avis 1" << std::endl;
+ CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
+ std::cout << "Avis 2" << std::endl;
+ /* process */
+ camera->process(image);
+ std::cout << "FEATURES:" << image->getFeatureList().size() << std::endl;
+ std::cout << "MATCHES:" << image->matches_from_precedent_.size() << std::endl;
- // Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 3D")==0 );
}
}
-}*/
+
+
+TEST(ProcessorBundleAdjustment, processKnown)
+{
+ std::cout << "EMPTY Test\n";
+}
+
+TEST(ProcessorBundleAdjustment, processNew)
+{
+ std::cout << "EMPTY Test\n";
+}
+
int main(int argc, char **argv)
--
GitLab
From 0a597a1a2ce2b8a212ae4d285875a703509f6124 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Wed, 22 May 2019 11:47:50 +0200
Subject: [PATCH 09/58] WIP
---
include/vision/factor/factor_pixelHP.h | 35 +-
src/landmark/landmark_HP.cpp | 2 +-
src/processor/processor_bundle_adjustment.cpp | 68 ++--
src/sensor/sensor_camera.cpp | 2 +-
test/CMakeLists.txt | 4 +
test/gtest_factor_autodiff_trifocal.cpp | 6 +-
test/gtest_factor_pixelHP.cpp | 340 ++++++++++++++++++
test/gtest_processor_bundle_adjustment.cpp | 76 +++-
8 files changed, 469 insertions(+), 64 deletions(-)
create mode 100644 test/gtest_factor_pixelHP.cpp
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
index 070ee1062..6e688febe 100644
--- a/include/vision/factor/factor_pixelHP.h
+++ b/include/vision/factor/factor_pixelHP.h
@@ -77,8 +77,9 @@ inline FactorPixelHP::FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO(),
_landmark_ptr->getP()),
- intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState()) //TODO: what to do with intrinsic?
+ intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState()) //TODO: intrinsic
{
+// std::cout << "FactorPixelHP::Constructor\n";
// obtain some intrinsics from provided sensor
distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
}
@@ -119,22 +120,33 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
Map<const Matrix<T, 3, 1> > p_w_r(_frame_p);
Translation<T, 3> t_w_r(p_w_r);
Map<const Quaternion<T> > q_w_r(_frame_o);
- TransformType T_W_R = t_w_r * q_w_r;
+ TransformType T_w_r = t_w_r * q_w_r;
// current robot to current camera transform
- CaptureBasePtr capture = this->getFeature()->getCapture();
- Translation<T, 3> t_r_c (capture->getSensorP()->getState().cast<T>());
- Quaternions q_r_c_s(Eigen::Vector4s(capture->getSensorO()->getState()));
- Quaternion<T> q_r_c = q_r_c_s.cast<T>();
- TransformType T_R_C = t_r_c * q_r_c;
+ Map<const Matrix<T, 3, 1> > p_r_c(_sensor_p);
+ Translation<T, 3> t_r_c(p_r_c);
+ Map<const Quaternion<T> > q_r_c(_sensor_o);
+ TransformType T_r_c = t_r_c * q_r_c;
+
+// CaptureBasePtr capture = this->getFeature()->getCapture();
+// Translation<T, 3> t_r_c (capture->getSensorP()->getState().cast<T>());
+// Quaternions q_r_c_s(Eigen::Vector4s(capture->getSensorO()->getState()));
+// Quaternion<T> q_r_c = q_r_c_s.cast<T>();
+// TransformType T_R_C = t_r_c * q_r_c;
// hmg point in current camera frame C
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg(_lmk_hmg);
- Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_R_C .inverse(Eigen::Affine)
- * T_W_R .inverse(Eigen::Affine)
+ Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_r_c .inverse(Eigen::Affine)
+ * T_w_r .inverse(Eigen::Affine)
* landmark_hmg;
- // lmk direction vector //TODO:
+ //std::cout << "p_w_r = \n\t" << _frame_p[0] << "\n\t" << _frame_p[1] << "\n\t" << _frame_p[2] << "\n";
+// std::cout << "q_w_r = \n\t" << _frame_o[0] << "\n\t" << _frame_o[1] << "\n\t" << _frame_o[2] << "\n\t" << _frame_o[3] << "\n";
+// std::cout << "p_r_c = \n\t" << _sensor_p[0] << "\n\t" << _sensor_p[1] << "\n\t" << _sensor_p[2] << "\n";
+// std::cout << "q_r_c = \n\t" << _sensor_o[0] << "\n\t" << _sensor_o[1] << "\n\t" << _sensor_o[2] << "\n\t" << _sensor_o[3] << "\n";
+ std::cout << "landmark_hmg_c = \n\t" << landmark_hmg_c(0) << "\n\t" << landmark_hmg_c(1) << "\n\t" << landmark_hmg_c(2) << "\n\t" << landmark_hmg_c(3) << "\n";
+
+ // lmk direction vector
Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c.head(3);
// camera parameters
@@ -144,6 +156,9 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
// project point and exit
Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
+
+ std::cout << "expectation = \n\t" << expectation(0) << "\n\t" << expectation(1) << "\n";
+
}
template<typename T>
diff --git a/src/landmark/landmark_HP.cpp b/src/landmark/landmark_HP.cpp
index 73cfc148d..dc3f3043c 100644
--- a/src/landmark/landmark_HP.cpp
+++ b/src/landmark/landmark_HP.cpp
@@ -37,7 +37,7 @@ Eigen::Vector3s LandmarkHP::point() const
{
using namespace Eigen;
- /* TODO:
+ /* TODO: done when creating the landmark
Transform<Scalar,3,Affine> T_w_r
= Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
* Quaternions(getAnchorFrame()->getO()->getState().data());
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index e2daf4031..88adbc708 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -95,8 +95,6 @@ void ProcessorBundleAdjustment::preProcess()
void ProcessorBundleAdjustment::postProcess()
{
- //TODO: check visualization
-
std::map<int,std::list<vision_utils::KeyPointEnhanced> > kp_enh_tracks;
for (auto const & feat_base : last_ptr_->getFeatureList())
@@ -158,7 +156,6 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
}
}
}
-// std::cout << "RUNNING trackFeatures()\n";
return _feature_correspondences.size();
}
@@ -200,9 +197,6 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
}
}
- std::cout << "RUNNING detectNewFeatures()\n";
-
-
return _features_last_out.size();
}
@@ -210,11 +204,7 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
bool ProcessorBundleAdjustment::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
{
- //TODO: Implement
-//// assert(_origin_feature!=nullptr);
-// assert( _last_feature!=nullptr);
-// assert(_incoming_feature!=nullptr);
-// std::cout << "RUNNING correctFeatureDrift()\n";
+ //TODO: Implement if needed
return true;
}
@@ -260,30 +250,19 @@ FactorBasePtr ProcessorBundleAdjustment::createFactor(FeatureBasePtr _feature_pt
LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _feature_ptr)
{
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
- //FrameBasePtr anchor_frame = getLast()->getFrame();
+ Eigen::Vector2s point2D = _feature_ptr->getMeasurement();
+
+ Eigen::Vector3s point3D;
+ point3D = pinhole::backprojectPoint(getSensor()->getIntrinsic()->getState(),(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(), point2D);
+ point3D.normalize();
- Eigen::Vector2s point2D;
- point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
- point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
//Scalar distance = params_bundle_adjustment_->distance; // arbitrary value
Scalar distance = 1;
Eigen::Vector4s vec_homogeneous;
-
- point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
-
- Eigen::Vector3s point3D;
- point3D.head<2>() = point2D;
- point3D(2) = 1;
-
- point3D.normalize();
-
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
-
//TODO: lmk from camera to world coordinate frame.
-
Transform<Scalar,3,Affine> T_w_r
= Translation<Scalar,3>(_feature_ptr->getFrame()->getP()->getState())
* Quaternions(_feature_ptr->getFrame()->getO()->getState().data());
@@ -294,6 +273,13 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
* T_r_c
* vec_homogeneous;
+
+ std::cout << "Point2D: " << point2D.transpose() << std::endl;
+ std::cout << "Point3D: " << point3D.transpose() << std::endl;
+ std::cout << "Hmg_c: " << vec_homogeneous.transpose() << std::endl;
+ std::cout << "Hmg_w: " << vec_homogeneous_w.transpose() << std::endl;
+ //vec_homogeneous_w = vec_homogeneous;
+
LandmarkHPPtr lmk_hp_ptr = std::make_shared<LandmarkHP>(vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
_feature_ptr->setLandmarkId(lmk_hp_ptr->id());
return lmk_hp_ptr;
@@ -310,12 +296,12 @@ void ProcessorBundleAdjustment::establishFactors()
FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_last = pair_trkid_pair.second.second;
- assert(feature_origin!=nullptr && "null feature origin");
- assert(feature_last!=nullptr && "null feature last");
- assert(feature_origin->getCapture()!=nullptr);
- assert(feature_last->getCapture()!=nullptr);
- assert(feature_last->getCapture()->getFrame()!=nullptr && feature_last->getCapture()->getFrame()->isKey());
- assert(feature_origin->getCapture()->getFrame()!=nullptr && feature_origin->getCapture()->getFrame()->isKey());
+// assert(feature_origin!=nullptr && "null feature origin");
+// assert(feature_last!=nullptr && "null feature last");
+// assert(feature_origin->getCapture()!=nullptr);
+// assert(feature_last->getCapture()!=nullptr);
+// assert(feature_last->getCapture()->getFrame()!=nullptr && feature_last->getCapture()->getFrame()->isKey());
+// assert(feature_origin->getCapture()->getFrame()!=nullptr && feature_origin->getCapture()->getFrame()->isKey());
if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
{
@@ -335,14 +321,16 @@ void ProcessorBundleAdjustment::establishFactors()
FactorPixelHPPtr fac_ptr_origin = std::make_shared<FactorPixelHP>(feature_origin, lmk_hp, shared_from_this());
if (fac_ptr_origin != nullptr) // factor links
{
- feature_origin->addFactor(fac_ptr_origin);
- lmk->addConstrainedBy(fac_ptr_origin);
+ feature_origin->addFactor(fac_ptr_origin);
+ fac_ptr_origin->link(feature_origin);
+ lmk->addConstrainedBy(fac_ptr_origin);
}
FactorPixelHPPtr fac_ptr_last = std::make_shared<FactorPixelHP>(feature_last, lmk_hp, shared_from_this());
if (fac_ptr_last != nullptr) // factor links
{
- feature_last->addFactor(fac_ptr_last);
- lmk->addConstrainedBy(fac_ptr_last);
+ feature_last->addFactor(fac_ptr_last);
+ fac_ptr_last->link(feature_last);
+ lmk->addConstrainedBy(fac_ptr_last);
}
}
else
@@ -354,11 +342,11 @@ void ProcessorBundleAdjustment::establishFactors()
FactorPixelHPPtr fac_ptr_last = std::make_shared<FactorPixelHP>(feature_last, lmk_hp, shared_from_this());
if (fac_ptr_last != nullptr) // factor links
{
- feature_last->addFactor(fac_ptr_last);
- lmk->addConstrainedBy(fac_ptr_last);
+ feature_last->addFactor(fac_ptr_last);
+ fac_ptr_last->link(feature_last);
+ lmk->addConstrainedBy(fac_ptr_last);
}
}
-
}
}
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 1d36cde10..9d5b9fcc5 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -12,7 +12,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsC
img_width_(_intrinsics.width), //
img_height_(_intrinsics.height), //
distortion_(_intrinsics.distortion), //
- correction_(distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
+ correction_(distortion_.size()==0 ? 0 : distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
using_raw_(true)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index da0968019..e7e76d38a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -24,3 +24,7 @@ target_link_libraries(gtest_processor_tracker_feature_trifocal ${PLUGIN_NAME} ${
# ProcessorBundleAdjustment test
wolf_add_gtest(gtest_processor_bundle_adjustment gtest_processor_bundle_adjustment.cpp)
target_link_libraries(gtest_processor_bundle_adjustment ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
+
+# FactorPixelHP test
+wolf_add_gtest(gtest_factor_pixelHP gtest_factor_pixelHP.cpp)
+target_link_libraries(gtest_factor_pixelHP ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
\ No newline at end of file
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 5312a2905..cad71af23 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -3,9 +3,9 @@
#include "core/utils/logging.h"
#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/processor/processor_tracker_feature_trifocal.h"
-#include "core/capture/capture_image.h"
-#include "core/factor/factor_autodiff_trifocal.h"
+#include "vision/processor/processor_tracker_feature_trifocal.h"
+#include "vision/capture/capture_image.h"
+#include "vision/factor/factor_autodiff_trifocal.h"
using namespace Eigen;
using namespace wolf;
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
new file mode 100644
index 000000000..beb8be0ac
--- /dev/null
+++ b/test/gtest_factor_pixelHP.cpp
@@ -0,0 +1,340 @@
+/*
+ * gtest_factor_pixelHP.cpp
+ *
+ * Created on: May 16, 2019
+ * Author: ovendrell
+ */
+#include "core/factor/factor_block_absolute.h"
+#include "core/factor/factor_quaternion_absolute.h"
+
+#include "utils_gtest.h"
+#include "vision/factor/factor_pixelHP.h"
+#include "vision/landmark/landmark_HP.h"
+#include "vision/capture/capture_image.h"
+#include "core/ceres_wrapper/ceres_manager.h"
+#include "vision/processor/processor_bundle_adjustment.h"
+#include "vision/internal/config.h"
+#include "core/math/pinhole_tools.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_VISION_ROOT_DIR;
+
+
+class FactorPixelHPTest : public testing::Test{
+ public:
+ Vector3s pos1, pos2, pos3, pos_cam, point;
+ Vector3s euler1, euler2, euler3, euler_cam;
+ Quaternions quat1, quat2, quat3, quat_cam;
+ Vector4s vquat1, vquat2, vquat3, vquat_cam; // quaternions as vectors
+ Vector7s pose1, pose2, pose3, pose_cam;
+ Vector4s lmkHP1;
+
+ ProblemPtr problem;
+ CeresManagerPtr ceres_manager;
+
+ SensorCameraPtr camera;
+ ProcessorBundleAdjustmentPtr proc_bundle_adjustment;
+
+ SensorBasePtr S;
+ FrameBasePtr F1, F2, F3;
+ CaptureImagePtr I1, I2, I3;
+ FeaturePointImagePtr f1, f2, f3;
+
+ LandmarkHPPtr L1;
+
+ FactorPixelHPPtr c1;
+ FactorPixelHPPtr c2;
+ FactorPixelHPPtr c3;
+
+ Scalar pixel_noise_std;
+
+ virtual ~FactorPixelHPTest()
+ {
+ std::cout << "destructor\n";
+ }
+
+ virtual void SetUp() override
+ {
+ // configuration
+ /*
+ * We have three robot poses, in three frames, with cameras C1, C2, C3
+ * looking towards the origin of coordinates:
+ *
+ * Z
+ * |
+ * ________ C3
+ * / | /
+ * --------- /| C2
+ * | / |
+ * |____________/_ | ___ Y
+ * / | /
+ * / | /
+ * --------- C1 |/
+ * | / |
+ * ---------
+ * /
+ * X
+ *
+ * Each robot pose is at one axis, facing the origin:
+ * F1: pos = (1,0,0), ori = (0,0,pi)
+ * F2: pos = (0,1,0), ori = (0,0,-pi/2)
+ * F3: pos = (0,0,1), ori = (0,pi/2,pi)
+ *
+ * The robot has a camera looking forward
+ * S: pos = (0,0,0), ori = (-pi/1, 0, -pi/1)
+ *
+ * There is a point at the origin
+ * P: pos = (0,0,0)
+ *
+ * The camera is canonical
+ * K = Id.
+ *
+ * Therefore, P projects exactly at the origin on each camera,
+ * creating three features:
+ * f1: p1 = (0,0)
+ * f2: p2 = (0,0)
+ * f3: p3 = (0,0)
+ *
+ * We form a Wolf tree with three frames, three captures,
+ * three features, one landmark and 3 factors:
+ *
+ * Frame F1, Capture C1, feature f1, factor c1
+ * Frame F2, Capture C2, feature f2, factor c2
+ * Frame F3, Capture C3, feature f3, factor c3
+ * Landmark L1
+ *
+ * The three frame poses F1, F2, F3, the camera pose S in the robot frame
+ * and the Landmark position are variables subject to optimization
+ *
+ * We perform a number of tests based on this configuration.
+ */
+
+ // all frames look to the origin
+ pos1 << 2, 1, 1;
+ pos2 << 1, 2, 1;
+ pos3 << 1, 1, 2;
+ euler1 << 0, 0 , M_PI ;
+ euler2 << 0, 0 , -M_PI_2 ;
+ euler3 << 0, M_PI_2, M_PI ;
+ quat1 = e2q(euler1);
+ quat2 = e2q(euler2);
+ quat3 = e2q(euler3);
+ vquat1 = quat1.coeffs();
+ vquat2 = quat2.coeffs();
+ vquat3 = quat3.coeffs();
+ pose1 << pos1, vquat1;
+ pose2 << pos2, vquat2;
+ pose3 << pos3, vquat3;
+ lmkHP1 << 1, 1, 1, 1/sqrt(3);
+
+ // camera at the robot origin looking forward
+ pos_cam << 0, 0, 0;
+ euler_cam << 0,0,0;//-M_PI_2, 0, -M_PI_2;
+ quat_cam = e2q(euler_cam);
+ vquat_cam = quat_cam.coeffs();
+ pose_cam << pos_cam, vquat_cam;
+
+ // Build problem
+ problem = Problem::create("PO", 3);
+ ceres::Solver::Options options;
+ ceres_manager = std::make_shared<CeresManager>(problem, options);
+
+ // Install sensor and processor
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
+ intr->width = 640;
+ intr->height = 480;
+ S = problem->installSensor("CAMERA", "camera", pose_cam, intr);
+ camera = std::static_pointer_cast<SensorCamera>(S);
+
+// ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+// params->delete_ambiguities = true;
+// params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+// params->pixel_noise_std = 1.0;
+// params->min_track_length_for_factor = 3;
+// params->voting_active = true;
+// params->max_new_features = 5;
+//
+// ProcessorBasePtr proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", camera, params);
+// proc_bundle_adjustment = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ // Add three viewpoints with frame, capture and feature
+ pixel_noise_std = 1.0; //params->pixel_noise_std;
+ Vector2s pix(0,0);
+ Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2));
+
+ // Point
+ cv::Point2f p = cv::Point2f(intr->width /2, intr->height/2);
+ cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
+
+ F1 = problem->emplaceFrame(KEY, pose1, 1.0);
+ I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
+ f1 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp, 0, des, pix_cov)); // pixel at origin
+
+ F2 = problem->emplaceFrame(KEY, pose2, 2.0);
+ I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1))));
+ f2 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp, 0, des, pix_cov)); // pixel at origin
+
+ F3 = problem->emplaceFrame(KEY, pose3, 3.0);
+ I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
+ f3 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp, 0, des, pix_cov)); // pixel at origin
+
+ //create Landmark -- with f1, f2 or f3
+ //LandmarkBasePtr lmk = proc_bundle_adjustment->createLandmark(f1);
+ L1 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP1, camera, des));
+// problem->addLandmark(L1);
+// lmk->link(problem->getMap());
+
+ // factors
+ c1 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f1, f1, L1, nullptr /*proc*/));
+ c2 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f2, f2, L1, nullptr /*proc*/));
+ c3 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f3, f3, L1, nullptr /*proc*/));
+ }
+};
+
+TEST(ProcessorFactorPixelHP, test)
+{
+ //Build problem
+ ProblemPtr problem_ptr = Problem::create("PO", 3);
+ CeresManagerPtr ceres_mgr = std::make_shared<CeresManager>(problem_ptr);
+
+ // Install sensor
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem_ptr->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem_ptr->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ // Frame
+ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+
+ // Capture
+ auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
+
+ // Feature
+ cv::Point2f p = cv::Point2f(240, 320);
+ cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8U);
+
+ FeaturePointImagePtr fea0 = std::make_shared<FeaturePointImage>(kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
+ fea0->setCapture(cap0);
+ cap0->addFeature(fea0);
+ fea0->link(cap0);
+
+ // Landmark
+ LandmarkBasePtr lmk = proc_bundle_adj->createLandmark(fea0);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+ problem_ptr->addLandmark(lmk_hp);
+ lmk->link(problem_ptr->getMap());
+
+ // Factor
+ auto fac0 = FactorBase::emplace<FactorPixelHP>(fea0, fea0, lmk_hp, proc);
+ auto fac_ptr = std::static_pointer_cast<FactorPixelHP>(fac0);
+
+ ASSERT_TRUE(problem_ptr->check(0));
+
+ //ASSERT the expectation of the landmark should be equal to the measurement
+ Eigen::VectorXs expect = fac_ptr->expectation();
+// std::cout << expect << std::endl;
+ ASSERT_FLOAT_EQ(expect(0,0),fea0->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),fea0->getMeasurement()(1,0));
+}
+
+TEST_F(FactorPixelHPTest, testSolveLandmark)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ S->fix();
+ F1->fix();
+ F2->fix();
+ F3->fix();
+ L1->unfix();
+
+ auto orig = L1->getState();
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << L1->getState().transpose() << std::endl;
+
+ ASSERT_MATRIX_APPROX(L1->getState(), orig, 1e-6);
+
+}
+
+TEST_F(FactorPixelHPTest, testSolveLandmarkAltered)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ S->fix();
+ F1->fix();
+ F2->fix();
+ F3->fix();
+ L1->unfix();
+
+ auto orig = L1->getState();
+ L1->getP()->setState(L1->getState() + Vector4s::Random());
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << L1->getState().transpose() << std::endl;
+
+ ASSERT_MATRIX_APPROX(L1->getState(), orig, 1e-6);
+
+}
+
+TEST_F(FactorPixelHPTest, testSolveFramePosition)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ S->fix();
+ F2->fix();
+ F3->fix();
+ L1->fix();
+
+ auto orig = F1->getP()->getState();
+
+ //change state
+ Vector3s position;
+ position << 1,0.2,0;
+ auto ori = F1->getO()->getState();
+ Vector7s state;
+ F1->setState(state);
+
+ F1->getO()->fix();
+ F1->getP()->unfix();
+
+ std::string brief_report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << F1->getP()->getState().transpose() << std::endl;
+
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index c87bc3d91..027597f13 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -5,6 +5,8 @@
#include "vision/sensor/sensor_camera.h"
#include "core/sensor/sensor_factory.h"
#include "vision/processor/processor_bundle_adjustment.h"
+#include "vision/factor/factor_pixelHP.h"
+#include "vision/landmark/landmark_HP.h"
#include "vision/capture/capture_image.h"
#include "vision/internal/config.h"
@@ -16,7 +18,6 @@
using namespace wolf;
-//std::string wolf_root = "/home/ovendrell/dev/vision";
std::string wolf_root = _WOLF_VISION_ROOT_DIR;
class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
@@ -219,7 +220,48 @@ TEST(ProcessorBundleAdjustment, establishFactors)
TEST(ProcessorBundleAdjustment, createLandmark)
{
- std::cout << "EMPTY Test\n";
+ //Build problem
+ ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+ // Install sensor
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem_ptr->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem_ptr->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ //Frame
+ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+
+ // Capture, feature and factor
+ auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
+
+ cv::Point2f p = cv::Point2f(240, 320);
+ cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8U);
+
+ FeaturePointImagePtr fea0 = std::make_shared<FeaturePointImage>(kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
+ fea0->setCapture(cap0);
+ cap0->addFeature(fea0);
+ fea0->link(cap0);
+
+ ASSERT_TRUE(problem_ptr->check(1));
+
+ LandmarkBasePtr lmk = proc_bundle_adj->createLandmark(fea0);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+ problem_ptr->addLandmark(lmk_hp);
+
+ ASSERT_EQ(problem_ptr->getMap()->getLandmarkList().size(), 1);
}
@@ -230,6 +272,7 @@ TEST(ProcessorBundleAdjustment, process)
// Install camera
IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1); //TODO: important, must be initialized
intr->width = 640;
intr->height = 480;
SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
@@ -274,6 +317,7 @@ TEST(ProcessorBundleAdjustment, process)
CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
proc_dummy->process(image_inc_ptr4);
ASSERT_EQ(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
+
}
TEST(ProcessorBundleAdjustment, processVideo)
@@ -283,6 +327,7 @@ TEST(ProcessorBundleAdjustment, processVideo)
// Install camera
IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
intr->width = 640;
intr->height = 480;
SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
@@ -295,13 +340,13 @@ TEST(ProcessorBundleAdjustment, processVideo)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 400;
- params->min_features_for_keyframe = 0;
+ params->min_features_for_keyframe = 50;
auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
//==================================vision_utils ============================================
vision_utils::SensorCameraPtr sen_ptr = std::make_shared<vision_utils::SensorCamera>();
- sen_ptr->open("/home/ovendrell/eclipse-workspace/ddm_triad/data/video/VID4b.mp4");
+ sen_ptr->open("/home/ovendrell/eclipse-workspace/ddm_triad/data/video/VID4b.mp4"); //TODO: this should be a demo
//"/home/ovendrell/dev/vision_utils/src/test/data/test_usb_cam.mp4");
unsigned int buffer_size = 10;
@@ -324,16 +369,29 @@ TEST(ProcessorBundleAdjustment, processVideo)
for(int frame_count = 0; frame_count<150; ++frame_count)
{
t += dt;
- std::cout << "Avis 0" << std::endl;
// Image ---------------------------------------------
frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
- std::cout << "Avis 1" << std::endl;
CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
- std::cout << "Avis 2" << std::endl;
/* process */
camera->process(image);
- std::cout << "FEATURES:" << image->getFeatureList().size() << std::endl;
- std::cout << "MATCHES:" << image->matches_from_precedent_.size() << std::endl;
+// std::cout << "FEATURES:" << image->getFeatureList().size() << std::endl;
+// std::cout << "MATCHES:" << image->matches_from_precedent_.size() << std::endl;
+
+ //Debug lines
+// if (frame_count > 4 && frame_count < 10)
+// {
+// FactorBasePtrList fac_list;
+// assert(image->getFeatureList().size()!=0);
+// image->getFactorList(fac_list);
+// assert(fac_list.size()!=0);
+// typedef std::list<FactorBasePtr>::iterator iter;
+// for (iter it =fac_list.begin(); it != fac_list.end(); ++it)
+// {
+// auto fac = std::static_pointer_cast<FactorPixelHP>(*it);
+// Eigen::VectorXs exp = fac->expectation();
+// std::cout << exp << std::endl;
+// }
+// }
}
}
--
GitLab
From eaa25aff31100092ac46d11190deed45916b3825 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Wed, 22 May 2019 12:58:25 +0200
Subject: [PATCH 10/58] CMake hotfix
---
CMakeLists.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5589b3c89..ebc11caaa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -149,7 +149,7 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
#INCLUDES SECTION
# ============EXAMPLE==================
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
include_directories(BEFORE "include")
--
GitLab
From e6e18301b1108f7a8077c8e5d175b1026c20e4b9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 13:24:13 +0200
Subject: [PATCH 11/58] Link against YAML lib
---
CMakeLists.txt | 2 ++
1 file changed, 2 insertions(+)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ebc11caaa..657b8444d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -279,6 +279,8 @@ ADD_LIBRARY(${PLUGIN_NAME}
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
+
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${OpenCV_LIBS})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
--
GitLab
From f5d346ed264b1129d57fd19ff5ffa1213c4f2975 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 13:24:33 +0200
Subject: [PATCH 12/58] Use the correct quotes / brackets in includes
---
include/vision/capture/capture_image.h | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h
index 08a934052..8b3ff0b0d 100644
--- a/include/vision/capture/capture_image.h
+++ b/include/vision/capture/capture_image.h
@@ -2,13 +2,13 @@
#define CAPTURE_IMAGE_H
//Wolf includes
-#include "core/capture/capture_base.h"
+#include <core/capture/capture_base.h>
#include "vision/feature/feature_point_image.h"
#include "vision/sensor/sensor_camera.h"
// Vision Utils includes
-#include "vision_utils/vision_utils.h"
-#include "vision_utils/common_class/frame.h"
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/common_class/frame.h>
namespace wolf {
--
GitLab
From cb86e7f3b6ddb9cc212cd60076813ea67099fcd5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 13:24:47 +0200
Subject: [PATCH 13/58] Add override keyword
---
include/vision/processor/processor_bundle_adjustment.h | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index 11e9b371b..8c55cdb5b 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -79,7 +79,7 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
- virtual void preProcess();
+ virtual void preProcess() override;
/** Post-process
*
@@ -91,7 +91,7 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
- virtual void postProcess();
+ virtual void postProcess() override;
/** \brief Track provided features from \b last to \b incoming
* \param _features_last_in input list of features in \b last to track
--
GitLab
From 084cd988b71e9406c5d7224603a2628b5b92ee26 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Wed, 22 May 2019 13:27:58 +0200
Subject: [PATCH 14/58] WIP
---
include/vision/factor/factor_pixelHP.h | 11 +++--
src/processor/processor_bundle_adjustment.cpp | 4 +-
test/gtest_factor_pixelHP.cpp | 46 ++++++++++++-------
3 files changed, 39 insertions(+), 22 deletions(-)
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
index 6e688febe..b2d90b40f 100644
--- a/include/vision/factor/factor_pixelHP.h
+++ b/include/vision/factor/factor_pixelHP.h
@@ -114,7 +114,7 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
using namespace Eigen;
// All involved transforms typedef
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
// world to current robot transform
Map<const Matrix<T, 3, 1> > p_w_r(_frame_p);
@@ -136,8 +136,8 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
// hmg point in current camera frame C
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg(_lmk_hmg);
- Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_r_c .inverse(Eigen::Affine)
- * T_w_r .inverse(Eigen::Affine)
+ Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_r_c .inverse(Eigen::Isometry)
+ * T_w_r .inverse(Eigen::Isometry)
* landmark_hmg;
//std::cout << "p_w_r = \n\t" << _frame_p[0] << "\n\t" << _frame_p[1] << "\n\t" << _frame_p[2] << "\n";
@@ -149,13 +149,16 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
// lmk direction vector
Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c.head(3);
+ // lmk inverse distance
+ T rho = landmark_hmg_c(3);
+
// camera parameters
Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
Eigen::Matrix<T, Eigen::Dynamic, 1> distortion = distortion_.cast<T>();
// project point and exit
Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
- expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
+ expectation = pinhole::projectPoint(intrinsic, distortion, v_dir/rho);
std::cout << "expectation = \n\t" << expectation(0) << "\n\t" << expectation(1) << "\n";
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 88adbc708..264959f90 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -263,10 +263,10 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
//TODO: lmk from camera to world coordinate frame.
- Transform<Scalar,3,Affine> T_w_r
+ Transform<Scalar,3,Isometry> T_w_r
= Translation<Scalar,3>(_feature_ptr->getFrame()->getP()->getState())
* Quaternions(_feature_ptr->getFrame()->getO()->getState().data());
- Transform<Scalar,3,Affine> T_r_c
+ Transform<Scalar,3,Isometry> T_r_c
= Translation<Scalar,3>(_feature_ptr->getCapture()->getSensorP()->getState())
* Quaternions(_feature_ptr->getCapture()->getSensorO()->getState().data());
Eigen::Matrix<Scalar, 4, 1> vec_homogeneous_w = T_w_r
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index beb8be0ac..4202cbf61 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -31,7 +31,7 @@ class FactorPixelHPTest : public testing::Test{
Quaternions quat1, quat2, quat3, quat_cam;
Vector4s vquat1, vquat2, vquat3, vquat_cam; // quaternions as vectors
Vector7s pose1, pose2, pose3, pose_cam;
- Vector4s lmkHP1;
+ Vector4s lmkHP1, lmkHP2, lmkHP3, lmkHP4;
ProblemPtr problem;
CeresManagerPtr ceres_manager;
@@ -45,6 +45,9 @@ class FactorPixelHPTest : public testing::Test{
FeaturePointImagePtr f1, f2, f3;
LandmarkHPPtr L1;
+ LandmarkHPPtr L2;
+ LandmarkHPPtr L3;
+ LandmarkHPPtr L4;
FactorPixelHPPtr c1;
FactorPixelHPPtr c2;
@@ -114,9 +117,9 @@ class FactorPixelHPTest : public testing::Test{
*/
// all frames look to the origin
- pos1 << 2, 1, 1;
- pos2 << 1, 2, 1;
- pos3 << 1, 1, 2;
+ pos1 << 1, 0, 0;
+ pos2 << 0, 1, 0;
+ pos3 << 0, 0, 1;
euler1 << 0, 0 , M_PI ;
euler2 << 0, 0 , -M_PI_2 ;
euler3 << 0, M_PI_2, M_PI ;
@@ -129,11 +132,16 @@ class FactorPixelHPTest : public testing::Test{
pose1 << pos1, vquat1;
pose2 << pos2, vquat2;
pose3 << pos3, vquat3;
- lmkHP1 << 1, 1, 1, 1/sqrt(3);
+
+ //landmarks
+ lmkHP1 << 0, 0, 0, 1;
+ lmkHP2 << 0, 0, 0, 1;
+ lmkHP3 << 0, 0, 0, 1;
+ lmkHP4 << 0, 0, 0, 1;
// camera at the robot origin looking forward
pos_cam << 0, 0, 0;
- euler_cam << 0,0,0;//-M_PI_2, 0, -M_PI_2;
+ euler_cam << -M_PI_2, 0, -M_PI_2;
quat_cam = e2q(euler_cam);
vquat_cam = quat_cam.coeffs();
pose_cam << pos_cam, vquat_cam;
@@ -145,7 +153,7 @@ class FactorPixelHPTest : public testing::Test{
// Install sensor and processor
IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
- intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
+ intr->pinhole_model_raw = Eigen::Vector4s(320,240,320,320);
intr->width = 640;
intr->height = 480;
S = problem->installSensor("CAMERA", "camera", pose_cam, intr);
@@ -197,7 +205,7 @@ class FactorPixelHPTest : public testing::Test{
}
};
-TEST(ProcessorFactorPixelHP, test)
+TEST(ProcessorFactorPixelHP, testZeroResidual)
{
//Build problem
ProblemPtr problem_ptr = Problem::create("PO", 3);
@@ -266,15 +274,15 @@ TEST_F(FactorPixelHPTest, testSolveLandmark)
F3->fix();
L1->unfix();
- auto orig = L1->getState();
+ auto orig = L1->point();
std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl;
std::cout << orig.transpose() << std::endl;
- std::cout << L1->getState().transpose() << std::endl;
+ std::cout << L1->point().transpose() << std::endl;
- ASSERT_MATRIX_APPROX(L1->getState(), orig, 1e-6);
+ ASSERT_MATRIX_APPROX(L1->point(), orig, 1e-6);
}
@@ -288,16 +296,16 @@ TEST_F(FactorPixelHPTest, testSolveLandmarkAltered)
F3->fix();
L1->unfix();
- auto orig = L1->getState();
+ auto orig = L1->point();
L1->getP()->setState(L1->getState() + Vector4s::Random());
std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl;
std::cout << orig.transpose() << std::endl;
- std::cout << L1->getState().transpose() << std::endl;
+ std::cout << L1->point().transpose() << std::endl;
- ASSERT_MATRIX_APPROX(L1->getState(), orig, 1e-6);
+ ASSERT_MATRIX_APPROX(L1->point(), orig, 1e-6);
}
@@ -322,12 +330,18 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
F1->getO()->fix();
F1->getP()->unfix();
- std::string brief_report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
std::cout << orig.transpose() << std::endl;
std::cout << F1->getP()->getState().transpose() << std::endl;
- ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+ //ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+
+ Eigen::VectorXs expect = c1->expectation();
+ ASSERT_FLOAT_EQ(expect(0,0),f1->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),f1->getMeasurement()(1,0));
}
--
GitLab
From 545105e4c96cb272d18e41c191f289bfc28155cf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 14:42:00 +0200
Subject: [PATCH 15/58] Remove debug couts
---
include/vision/factor/factor_pixelHP.h | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
index b2d90b40f..73279f4b7 100644
--- a/include/vision/factor/factor_pixelHP.h
+++ b/include/vision/factor/factor_pixelHP.h
@@ -144,7 +144,7 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
// std::cout << "q_w_r = \n\t" << _frame_o[0] << "\n\t" << _frame_o[1] << "\n\t" << _frame_o[2] << "\n\t" << _frame_o[3] << "\n";
// std::cout << "p_r_c = \n\t" << _sensor_p[0] << "\n\t" << _sensor_p[1] << "\n\t" << _sensor_p[2] << "\n";
// std::cout << "q_r_c = \n\t" << _sensor_o[0] << "\n\t" << _sensor_o[1] << "\n\t" << _sensor_o[2] << "\n\t" << _sensor_o[3] << "\n";
- std::cout << "landmark_hmg_c = \n\t" << landmark_hmg_c(0) << "\n\t" << landmark_hmg_c(1) << "\n\t" << landmark_hmg_c(2) << "\n\t" << landmark_hmg_c(3) << "\n";
+// std::cout << "landmark_hmg_c = \n\t" << landmark_hmg_c(0) << "\n\t" << landmark_hmg_c(1) << "\n\t" << landmark_hmg_c(2) << "\n\t" << landmark_hmg_c(3) << "\n";
// lmk direction vector
Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c.head(3);
@@ -160,7 +160,7 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
expectation = pinhole::projectPoint(intrinsic, distortion, v_dir/rho);
- std::cout << "expectation = \n\t" << expectation(0) << "\n\t" << expectation(1) << "\n";
+// std::cout << "expectation = \n\t" << expectation(0) << "\n\t" << expectation(1) << "\n";
}
--
GitLab
From 89699b85514341ae6f3208cfa8e31b5a118625cb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 14:42:17 +0200
Subject: [PATCH 16/58] Fix test of frame to only 2DoF -> PASS.
---
test/gtest_factor_pixelHP.cpp | 9 +++++++--
1 file changed, 7 insertions(+), 2 deletions(-)
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 4202cbf61..24bafc6e6 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -322,9 +322,10 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
//change state
Vector3s position;
- position << 1,0.2,0;
+ position << 2.0, 2.0, 2.0;
auto ori = F1->getO()->getState();
Vector7s state;
+ state << position, ori;
F1->setState(state);
F1->getO()->fix();
@@ -337,7 +338,11 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
std::cout << orig.transpose() << std::endl;
std::cout << F1->getP()->getState().transpose() << std::endl;
- //ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+ // This test is no good because it checks 3 DoF and only 2DoF are observable.
+ //ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+ // We use the following alternative:
+ // Frame must be in the X axis, so Y=0 and Z=0
+ ASSERT_MATRIX_APPROX(F1->getP()->getState().tail(2), orig.tail(2), 1e-6);
Eigen::VectorXs expect = c1->expectation();
ASSERT_FLOAT_EQ(expect(0,0),f1->getMeasurement()(0,0));
--
GitLab
From cf1a39f7df36432ba41f11fed00a80de7c94d310 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 19:50:51 +0200
Subject: [PATCH 17/58] Arrange includes
---
src/landmark/landmark_HP.cpp | 2 +-
test/gtest_factor_pixelHP.cpp | 11 +++++------
test/gtest_processor_bundle_adjustment.cpp | 21 +++++++++++----------
3 files changed, 17 insertions(+), 17 deletions(-)
diff --git a/src/landmark/landmark_HP.cpp b/src/landmark/landmark_HP.cpp
index dc3f3043c..a9bf53f4d 100644
--- a/src/landmark/landmark_HP.cpp
+++ b/src/landmark/landmark_HP.cpp
@@ -6,7 +6,7 @@
namespace wolf {
-/* Landmark - Anchored Homogeneous Point*/
+/* Landmark - Homogeneous Point*/
LandmarkHP::LandmarkHP(Eigen::Vector4s _position_homogeneous,
SensorBasePtr _sensor,
cv::Mat _2D_descriptor) :
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 24bafc6e6..b3a4b824c 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -4,19 +4,18 @@
* Created on: May 16, 2019
* Author: ovendrell
*/
-#include "core/factor/factor_block_absolute.h"
-#include "core/factor/factor_quaternion_absolute.h"
-
#include "utils_gtest.h"
#include "vision/factor/factor_pixelHP.h"
#include "vision/landmark/landmark_HP.h"
#include "vision/capture/capture_image.h"
-#include "core/ceres_wrapper/ceres_manager.h"
#include "vision/processor/processor_bundle_adjustment.h"
#include "vision/internal/config.h"
-#include "core/math/pinhole_tools.h"
-#include "core/ceres_wrapper/ceres_manager.h"
+#include <core/factor/factor_block_absolute.h>
+#include <core/factor/factor_quaternion_absolute.h>
+#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/math/pinhole_tools.h>
+#include <core/ceres_wrapper/ceres_manager.h>
using namespace wolf;
using namespace Eigen;
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 027597f13..10b7da713 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -1,20 +1,21 @@
-#include "utils_gtest.h"
-#include "vision_utils.h"
-
+// this plugin includes
#include "vision/sensor/sensor_camera.h"
-#include "core/sensor/sensor_factory.h"
#include "vision/processor/processor_bundle_adjustment.h"
#include "vision/factor/factor_pixelHP.h"
#include "vision/landmark/landmark_HP.h"
#include "vision/capture/capture_image.h"
#include "vision/internal/config.h"
+#include "utils_gtest.h"
+
+// wolf includes
+#include <core/sensor/sensor_factory.h>
// Vision utils includes
-#include <vision_utils.h>
-#include <sensors.h>
-#include <common_class/buffer.h>
-#include <common_class/frame.h>
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/sensors.h>
+#include <vision_utils/common_class/buffer.h>
+#include <vision_utils/common_class/frame.h>
using namespace wolf;
@@ -113,7 +114,7 @@ TEST(ProcessorBundleAdjustment, installProcessor)
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
ASSERT_EQ(proc->getProblem(), problem);
- ASSERT_TRUE(problem->check());
+ ASSERT_TRUE(problem->check(0));
}
TEST(ProcessorBundleAdjustment, preProcess)
@@ -255,7 +256,7 @@ TEST(ProcessorBundleAdjustment, createLandmark)
cap0->addFeature(fea0);
fea0->link(cap0);
- ASSERT_TRUE(problem_ptr->check(1));
+ ASSERT_TRUE(problem_ptr->check(0));
LandmarkBasePtr lmk = proc_bundle_adj->createLandmark(fea0);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
--
GitLab
From da1b7c8575b90626d2813330c239029294425b3c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 19:54:10 +0200
Subject: [PATCH 18/58] Do not compile processor_tracker_feature_image
---
CMakeLists.txt | 4 ++--
include/vision/processor/processor_tracker_feature_image.h | 2 +-
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 657b8444d..0c24864ba 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -187,7 +187,7 @@ SET(HDRS_PROCESSOR
include/vision/processor/processor_tracker_feature_trifocal.h
include/vision/processor/processor_params_image.h
include/vision/processor/processor_tracker_landmark_image.h
-include/vision/processor/processor_tracker_feature_image.h
+#include/vision/processor/processor_tracker_feature_image.h
include/vision/processor/processor_bundle_adjustment.h
)
SET(HDRS_SENSOR
@@ -220,7 +220,7 @@ src/landmark/landmark_point_3D.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_tracker_feature_trifocal.cpp
-src/processor/processor_tracker_feature_image.cpp
+#src/processor/processor_tracker_feature_image.cpp
src/processor/processor_tracker_landmark_image.cpp
src/processor/processor_bundle_adjustment.cpp
)
diff --git a/include/vision/processor/processor_tracker_feature_image.h b/include/vision/processor/processor_tracker_feature_image.h
index 04c68b619..f65deb9bc 100644
--- a/include/vision/processor/processor_tracker_feature_image.h
+++ b/include/vision/processor/processor_tracker_feature_image.h
@@ -8,7 +8,7 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/processor/processor_tracker_feature.h"
-#include "core/factor/factor_epipolar.h"
+#include "core/factor/factor_feature_dummy.h"
#include "vision/processor/processor_params_image.h"
// vision_utils
--
GitLab
From 512e66b5a34e9093bf10eaa4ec84c5e6c23e0738 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 22 May 2019 21:21:03 +0200
Subject: [PATCH 19/58] Rationalize cmakelists.txt a bit
---
CMakeLists.txt | 23 ++++++++++++-----------
1 file changed, 12 insertions(+), 11 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c24864ba..00c9648d2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -99,7 +99,10 @@ FIND_PACKAGE(Threads REQUIRED)
FIND_PACKAGE(wolf REQUIRED)
FIND_PACKAGE(vision_utils REQUIRED)
-MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
+IF (vision_utils_FOUND)
+ MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
+ENDIF(vision_utils_FOUND)
+
SET(PRINT_INFO_VU false)
FIND_PACKAGE(OpenCV REQUIRED)
@@ -276,26 +279,24 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_WRAPPER}
${SRCS_YAML}
)
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARY})
-
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
-
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${OpenCV_LIBS})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
#Link the created libraries
#===============EXAMPLE=========================
# IF (Ceres_FOUND)
# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
# ENDIF(Ceres_FOUND)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${OpenCV_LIBS})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
+IF (GLOG_FOUND)
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${GLOG_LIBRARY})
+ENDIF (GLOG_FOUND)
#Build tests
#===============EXAMPLE=========================
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
IF (GLOG_FOUND)
IF(BUILD_TESTS)
MESSAGE("Building tests.")
--
GitLab
From 6ce4fb4b95327505af43b5ef35a34f42dbbe150d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 00:25:36 +0200
Subject: [PATCH 20/58] Add dylib to lib extensions
---
cmake_modules/wolfvisionConfig.cmake | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/cmake_modules/wolfvisionConfig.cmake b/cmake_modules/wolfvisionConfig.cmake
index ba2464a95..64eb5b3f0 100644
--- a/cmake_modules/wolfvisionConfig.cmake
+++ b/cmake_modules/wolfvisionConfig.cmake
@@ -11,7 +11,7 @@ ENDIF(wolfvision_INCLUDE_DIR)
FIND_LIBRARY(
wolfvision_LIBRARY
- NAMES libwolfvision.so
+ NAMES libwolfvision.so libwolfvision.dylib
PATHS /usr/local/lib/iri-algorithms)
IF(wolfvision_LIBRARY)
MESSAGE("Found vision lib: ${wolfvision_LIBRARY}")
--
GitLab
From 0c52145f6b6c804118e3451f4d303b12c7dd17bb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 00:26:16 +0200
Subject: [PATCH 21/58] Comment out suitesparse stuff
---
CMakeLists.txt | 30 +++++++++++++++---------------
1 file changed, 15 insertions(+), 15 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 00c9648d2..b78024e7b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -118,19 +118,19 @@ ELSEIF(YAMLCPP_FOUND)
MESSAGE("yaml-cpp Library NOT FOUND!")
ENDIF(YAMLCPP_FOUND)
-FIND_PATH(
- Suitesparse_INCLUDE_DIRS
- NAMES SuiteSparse_config.h
- PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND TRUE)
- MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND FALSE)
- MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
+#FIND_PATH(
+# Suitesparse_INCLUDE_DIRS
+# NAMES SuiteSparse_config.h
+# PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
+#MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
+#
+#IF(Suitesparse_INCLUDE_DIRS)
+# SET(Suitesparse_FOUND TRUE)
+# MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
+#ELSE (Suitesparse_INCLUDE_DIRS)
+# SET(Suitesparse_FOUND FALSE)
+# MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
+#ENDIF (Suitesparse_INCLUDE_DIRS)
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -336,8 +336,8 @@ INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
+#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+# DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
INSTALL(FILES ${HDRS_SERIALIZATION}
--
GitLab
From 83f75cf97fed0481666e6f139586078e9e7b99b6 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Thu, 23 May 2019 13:16:55 +0200
Subject: [PATCH 22/58] add test to gtest factor_pixelHP
---
include/vision/factor/factor_pixelHP.h | 6 --
src/processor/processor_bundle_adjustment.cpp | 8 +-
src/sensor/sensor_camera.cpp | 2 +-
test/gtest_factor_pixelHP.cpp | 98 +++++++++++++++++--
test/gtest_processor_bundle_adjustment.cpp | 78 +--------------
5 files changed, 98 insertions(+), 94 deletions(-)
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
index 73279f4b7..c1515f9b1 100644
--- a/include/vision/factor/factor_pixelHP.h
+++ b/include/vision/factor/factor_pixelHP.h
@@ -128,12 +128,6 @@ inline void FactorPixelHP::expectation(const T* const _frame_p,
Map<const Quaternion<T> > q_r_c(_sensor_o);
TransformType T_r_c = t_r_c * q_r_c;
-// CaptureBasePtr capture = this->getFeature()->getCapture();
-// Translation<T, 3> t_r_c (capture->getSensorP()->getState().cast<T>());
-// Quaternions q_r_c_s(Eigen::Vector4s(capture->getSensorO()->getState()));
-// Quaternion<T> q_r_c = q_r_c_s.cast<T>();
-// TransformType T_R_C = t_r_c * q_r_c;
-
// hmg point in current camera frame C
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg(_lmk_hmg);
Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_r_c .inverse(Eigen::Isometry)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 264959f90..0c4b8a0c4 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -274,10 +274,10 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
* vec_homogeneous;
- std::cout << "Point2D: " << point2D.transpose() << std::endl;
- std::cout << "Point3D: " << point3D.transpose() << std::endl;
- std::cout << "Hmg_c: " << vec_homogeneous.transpose() << std::endl;
- std::cout << "Hmg_w: " << vec_homogeneous_w.transpose() << std::endl;
+// std::cout << "Point2D: " << point2D.transpose() << std::endl;
+// std::cout << "Point3D: " << point3D.transpose() << std::endl;
+// std::cout << "Hmg_c: " << vec_homogeneous.transpose() << std::endl;
+// std::cout << "Hmg_w: " << vec_homogeneous_w.transpose() << std::endl;
//vec_homogeneous_w = vec_homogeneous;
LandmarkHPPtr lmk_hp_ptr = std::make_shared<LandmarkHP>(vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 9d5b9fcc5..00bb895fa 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -12,7 +12,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsC
img_width_(_intrinsics.width), //
img_height_(_intrinsics.height), //
distortion_(_intrinsics.distortion), //
- correction_(distortion_.size()==0 ? 0 : distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
+ correction_(distortion_.size()==0 ? 0 : distortion_.size()), // make correction vector slightly larger in size than the distortion vector
pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
using_raw_(true)
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index b3a4b824c..b3f517911 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -42,13 +42,14 @@ class FactorPixelHPTest : public testing::Test{
FrameBasePtr F1, F2, F3;
CaptureImagePtr I1, I2, I3;
FeaturePointImagePtr f1, f2, f3;
+ FeaturePointImagePtr f12, f13, f14;
- LandmarkHPPtr L1;
+ LandmarkHPPtr L1; //only one landmark L1 is initialized
LandmarkHPPtr L2;
LandmarkHPPtr L3;
LandmarkHPPtr L4;
- FactorPixelHPPtr c1;
+ FactorPixelHPPtr c1, c12, c13, c14;
FactorPixelHPPtr c2;
FactorPixelHPPtr c3;
@@ -134,9 +135,6 @@ class FactorPixelHPTest : public testing::Test{
//landmarks
lmkHP1 << 0, 0, 0, 1;
- lmkHP2 << 0, 0, 0, 1;
- lmkHP3 << 0, 0, 0, 1;
- lmkHP4 << 0, 0, 0, 1;
// camera at the robot origin looking forward
pos_cam << 0, 0, 0;
@@ -308,7 +306,7 @@ TEST_F(FactorPixelHPTest, testSolveLandmarkAltered)
}
-TEST_F(FactorPixelHPTest, testSolveFramePosition)
+TEST_F(FactorPixelHPTest, testSolveFramePosition2ObservableDoF)
{
ASSERT_TRUE(problem->check(0));
@@ -349,6 +347,94 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
}
+TEST_F(FactorPixelHPTest, testSolveFramePosition)
+{
+ //landmark homogeneous coordinates in world reference
+ lmkHP2 << 0.5, 0.5, 0.5, 1;
+ lmkHP3 << -0.5, 0.75, 0.5, 1;
+ lmkHP4 << 0.5, 0, -0.5, 1;
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(F1->getP()->getState())
+ * Quaternions(F1->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(I1->getSensorP()->getState())
+ * Quaternions(I1->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c = lmkHP2_c.head<3>()/lmkHP2_c(3);
+ Vector3s lmk_point_3_c = lmkHP3_c.head<3>()/lmkHP3_c(3);
+ Vector3s lmk_point_4_c = lmkHP4_c.head<3>()/lmkHP4_c(3);
+
+ //points projected to image plane
+ Vector2s point2 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c);
+ Vector2s point3 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c);
+ Vector2s point4 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c);
+ cv::KeyPoint kp2 = cv::KeyPoint(cv::Point2f(point2(0),point2(1)), 32.0f);
+ cv::KeyPoint kp3 = cv::KeyPoint(cv::Point2f(point3(0),point3(1)), 32.0f);
+ cv::KeyPoint kp4 = cv::KeyPoint(cv::Point2f(point4(0),point4(1)), 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
+ pixel_noise_std = 1.0; //params->pixel_noise_std;
+ Vector2s pix(0,0);
+ Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2));
+
+ //create features
+ f12 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp2, 0, des, pix_cov));
+ f13 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp3, 0, des, pix_cov));
+ f14 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp4, 0, des, pix_cov));
+
+ //create landmarks
+ L2 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP2, camera, des));
+ L3 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP3, camera, des));
+ L4 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP4, camera, des));
+
+ //create factors
+ c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/));
+ c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/));
+ c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/));
+
+ ASSERT_TRUE(problem->check());
+
+ S->fix();
+ F2->fix();
+ F3->fix();
+ L1->fix();
+ L2->fix();
+ L3->fix();
+ L4->fix();
+
+ auto orig = F1->getP()->getState();
+
+ //change state
+ Vector3s position;
+ position << Vector3s::Random()*100;//2.0, 2.0, 2.0;
+ auto ori = F1->getO()->getState();
+ Vector7s state;
+ state << position, ori;
+ F1->setState(state);
+
+ F1->getO()->fix();
+ F1->getP()->unfix();
+
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << F1->getP()->getState().transpose() << std::endl;
+
+ // This test checks 3 DoF (3DoF are observable).
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+
+ Eigen::VectorXs expect = c1->expectation();
+ ASSERT_FLOAT_EQ(expect(0,0),f1->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),f1->getMeasurement()(1,0));
+}
+
int main(int argc, char **argv)
{
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 10b7da713..02e296bda 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -273,7 +273,7 @@ TEST(ProcessorBundleAdjustment, process)
// Install camera
IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
- intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1); //TODO: important, must be initialized
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1); //TODO: initialize
intr->width = 640;
intr->height = 480;
SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
@@ -321,82 +321,6 @@ TEST(ProcessorBundleAdjustment, process)
}
-TEST(ProcessorBundleAdjustment, processVideo)
-{
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 3);
-
- // Install camera
- IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
- intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
- intr->width = 640;
- intr->height = 480;
- SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
-
- // Install processor
- ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
- params->delete_ambiguities = false;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
- params->pixel_noise_std = 1.0;
- params->min_track_length_for_factor = 3;
- params->voting_active = true;
- params->max_new_features = 400;
- params->min_features_for_keyframe = 50;
- auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
- auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
-
- //==================================vision_utils ============================================
- vision_utils::SensorCameraPtr sen_ptr = std::make_shared<vision_utils::SensorCamera>();
- sen_ptr->open("/home/ovendrell/eclipse-workspace/ddm_triad/data/video/VID4b.mp4"); //TODO: this should be a demo
- //"/home/ovendrell/dev/vision_utils/src/test/data/test_usb_cam.mp4");
-
- unsigned int buffer_size = 10;
- vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
-
- unsigned int img_width = frame_buff.back()->getImage().cols;
- unsigned int img_height = frame_buff.back()->getImage().rows;
- std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
- //==================================vision_utils ============================================
-
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
- camera->setImgWidth(img_width);
- camera->setImgHeight(img_height);
- ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
-
- TimeStamp t = 0;
- // main loop
- Scalar dt = 0.04;
- for(int frame_count = 0; frame_count<150; ++frame_count)
- {
- t += dt;
- // Image ---------------------------------------------
- frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
- CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
- /* process */
- camera->process(image);
-// std::cout << "FEATURES:" << image->getFeatureList().size() << std::endl;
-// std::cout << "MATCHES:" << image->matches_from_precedent_.size() << std::endl;
-
- //Debug lines
-// if (frame_count > 4 && frame_count < 10)
-// {
-// FactorBasePtrList fac_list;
-// assert(image->getFeatureList().size()!=0);
-// image->getFactorList(fac_list);
-// assert(fac_list.size()!=0);
-// typedef std::list<FactorBasePtr>::iterator iter;
-// for (iter it =fac_list.begin(); it != fac_list.end(); ++it)
-// {
-// auto fac = std::static_pointer_cast<FactorPixelHP>(*it);
-// Eigen::VectorXs exp = fac->expectation();
-// std::cout << exp << std::endl;
-// }
-// }
-
- }
-}
-
TEST(ProcessorBundleAdjustment, processKnown)
{
--
GitLab
From 2d153257121e51f0fb71211145e98cf1e0bd4a86 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Thu, 23 May 2019 13:30:50 +0200
Subject: [PATCH 23/58] yaml bundle adjustment update
---
...cessor_bundle_adjustment_vision_utils.yaml | 36 ++++++++++---------
1 file changed, 19 insertions(+), 17 deletions(-)
diff --git a/demos/processor_bundle_adjustment_vision_utils.yaml b/demos/processor_bundle_adjustment_vision_utils.yaml
index 7621cd8ad..305f609b2 100644
--- a/demos/processor_bundle_adjustment_vision_utils.yaml
+++ b/demos/processor_bundle_adjustment_vision_utils.yaml
@@ -2,30 +2,32 @@ sensor:
type: "USB_CAM"
detector:
- type: "GFTT"
- maxCorners: 1000
- qualityLevel: 0.01
- minDistance: 1.0
- blockSize: 3
- k: 0.04
+ type: "ORB"
+ nfeatures: 400
+ scale factor: 1.0
+ nlevels: 1
+ edge threshold: 31 # 16
+ first level: 0
+ WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
+ score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
+ patch size: 31 # 31
descriptor:
type: "ORB"
- nfeatures: 1000
- scale factor: 1.2
+ nfeatures: 400
+ scale factor: 1.0
nlevels: 1
- edge threshold: 8 # 16
+ edge threshold: 31 # 16
first level: 0
WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
+ patch size: 31 # 31
+
matcher:
- type: "BRUTEFORCE_HAMMING" # BRUTEFORCE, BRUTEFORCE_HAMMING, BRUTEFORCE_HAMMING_2, BRUTEFORCE_L1, FLANNBASED
- match type: 2 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
- min normalized score: 0.9
+ type: "FLANNBASED" # BRUTEFORCE, BRUTEFORCE_HAMMING, BRUTEFORCE_HAMMING_2, BRUTEFORCE_L1, FLANNBASED
+ match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
filtering:
- ransac epipolar distance: 1 # Distance to epipolar [pixels]
- ransac confidence prob: 0.98 # Confidence probability
- max match euclidean dist: 50 # Max euclidean distance to consider a match as inlier
+ ransac epipolar distance: 3 # Distance to epipolar [pixels]
+ ransac confidence prob: 0.97 # Confidence probability
+ max match euclidean dist: 20 # Max euclidean distance to consider a match as inlier
\ No newline at end of file
--
GitLab
From f5d469b13b9f14200f9c2700f52b06e991557e4f Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Thu, 23 May 2019 14:32:31 +0200
Subject: [PATCH 24/58] Fixes after merging from devel
---
CMakeLists.txt | 19 ++++++++++---------
.../processor_tracker_feature_image.cpp | 4 ++--
test/gtest_factor_pixelHP.cpp | 2 +-
test/gtest_processor_bundle_adjustment.cpp | 2 +-
4 files changed, 14 insertions(+), 13 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5589b3c89..d00762c84 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -103,18 +103,18 @@ MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
SET(PRINT_INFO_VU false)
FIND_PACKAGE(OpenCV REQUIRED)
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
+FIND_PACKAGE(YamlCpp REQUIRED)
IF(YAMLCPP_FOUND)
MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
ELSEIF(YAMLCPP_FOUND)
MESSAGE("yaml-cpp Library NOT FOUND!")
ENDIF(YAMLCPP_FOUND)
+FIND_PACKAGE(Ceres QUIET) #Ceres is not required
+IF(Ceres_FOUND)
+ MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
+ENDIF(Ceres_FOUND)
+
FIND_PATH(
Suitesparse_INCLUDE_DIRS
NAMES SuiteSparse_config.h
@@ -149,10 +149,11 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
#INCLUDES SECTION
# ============EXAMPLE==================
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-include_directories(BEFORE "include")
+INCLUDE_DIRECTORIES(BEFORE "include")
+INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
IF(Ceres_FOUND)
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
ENDIF(Ceres_FOUND)
@@ -278,7 +279,7 @@ ADD_LIBRARY(${PLUGIN_NAME}
)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARY})
-
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${OpenCV_LIBS})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index ecc8a249b..ab7c59b19 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -250,11 +250,11 @@ Scalar ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _
FactorBasePtr ProcessorTrackerFeatureImage::createFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- FactorEpipolarPtr const_epipolar_ptr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr,
+ FactorFeatureDummyPtr const_dummy_ptr = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr,
shared_from_this());
// _feature_ptr->addFactor(const_epipolar_ptr);
// _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
- return const_epipolar_ptr;
+ return const_dummy_ptr;
}
unsigned int ProcessorTrackerFeatureImage::detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index b3f517911..b05ba9f7c 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -4,12 +4,12 @@
* Created on: May 16, 2019
* Author: ovendrell
*/
-#include "utils_gtest.h"
#include "vision/factor/factor_pixelHP.h"
#include "vision/landmark/landmark_HP.h"
#include "vision/capture/capture_image.h"
#include "vision/processor/processor_bundle_adjustment.h"
#include "vision/internal/config.h"
+#include <core/utils/utils_gtest.h>
#include <core/factor/factor_block_absolute.h>
#include <core/factor/factor_quaternion_absolute.h>
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 02e296bda..77a7bcfcc 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -6,7 +6,7 @@
#include "vision/landmark/landmark_HP.h"
#include "vision/capture/capture_image.h"
#include "vision/internal/config.h"
-#include "utils_gtest.h"
+#include <core/utils/utils_gtest.h>
// wolf includes
#include <core/sensor/sensor_factory.h>
--
GitLab
From 63aa5e78c4211135d644220bf99d8ef532973dc7 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 17:27:24 +0200
Subject: [PATCH 25/58] Fix pinhole_tools.h include and organize includes
---
include/vision/factor/factor_AHP.h | 6 +++---
include/vision/factor/factor_pixelHP.h | 6 +++---
.../processor_tracker_landmark_image.cpp | 15 ++++++++-------
src/sensor/sensor_camera.cpp | 3 ++-
test/gtest_factor_pixelHP.cpp | 4 ++--
test/gtest_pinhole.cpp | 3 ++-
test/gtest_processor_bundle_adjustment.cpp | 3 ++-
test/gtest_processor_tracker_feature_trifocal.cpp | 14 +++++++-------
8 files changed, 29 insertions(+), 25 deletions(-)
diff --git a/include/vision/factor/factor_AHP.h b/include/vision/factor/factor_AHP.h
index c0f080367..3ef1b6c8b 100644
--- a/include/vision/factor/factor_AHP.h
+++ b/include/vision/factor/factor_AHP.h
@@ -2,11 +2,11 @@
#define FACTOR_AHP_H
//Wolf includes
-#include "core/factor/factor_autodiff.h"
#include "vision/landmark/landmark_AHP.h"
#include "vision/sensor/sensor_camera.h"
-//#include "core/feature/feature_point_image.h"
-#include "core/math/pinhole_tools.h"
+#include "vision/math/pinhole_tools.h"
+
+#include "core/factor/factor_autodiff.h"
#include <iomanip> //setprecision
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
index c1515f9b1..6ade3a3b1 100644
--- a/include/vision/factor/factor_pixelHP.h
+++ b/include/vision/factor/factor_pixelHP.h
@@ -2,11 +2,11 @@
#define FACTOR_AHP_H
//Wolf includes
-#include "core/factor/factor_autodiff.h"
#include "vision/landmark/landmark_HP.h"
#include "vision/sensor/sensor_camera.h"
-//#include "core/feature/feature_point_image.h"
-#include "core/math/pinhole_tools.h"
+#include "vision/math/pinhole_tools.h"
+
+#include "core/factor/factor_autodiff.h"
#include <iomanip> //setprecision
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index 0636048dd..b9ae15084 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -2,22 +2,23 @@
#include "vision/capture/capture_image.h"
#include "vision/factor/factor_AHP.h"
-#include "core/feature/feature_base.h"
#include "vision/feature/feature_point_image.h"
+#include "vision/math/pinhole_tools.h"
+#include "vision/sensor/sensor_camera.h"
+
+#include "core/feature/feature_base.h"
#include "core/frame/frame_base.h"
#include "core/utils/logging.h"
#include "core/map/map_base.h"
-#include "core/math/pinhole_tools.h"
#include "core/problem/problem.h"
-#include "vision/sensor/sensor_camera.h"
#include "core/state_block/state_block.h"
#include "core/common/time_stamp.h"
// vision_utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
+#include <vision_utils/detectors.h>
+#include <vision_utils/descriptors.h>
+#include <vision_utils/matchers.h>
+#include <vision_utils/algorithms.h>
#include <Eigen/Geometry>
#include <iomanip> //setprecision
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 00bb895fa..39386d4d2 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -1,6 +1,7 @@
+
#include "vision/sensor/sensor_camera.h"
+#include "vision/math/pinhole_tools.h"
-#include "core/math/pinhole_tools.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index b05ba9f7c..59647ad9f 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -9,12 +9,12 @@
#include "vision/capture/capture_image.h"
#include "vision/processor/processor_bundle_adjustment.h"
#include "vision/internal/config.h"
-#include <core/utils/utils_gtest.h>
+#include "vision/math/pinhole_tools.h"
+#include <core/utils/utils_gtest.h>
#include <core/factor/factor_block_absolute.h>
#include <core/factor/factor_quaternion_absolute.h>
#include <core/ceres_wrapper/ceres_manager.h>
-#include <core/math/pinhole_tools.h>
#include <core/ceres_wrapper/ceres_manager.h>
using namespace wolf;
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
index bcc76f5fa..9e71295ac 100644
--- a/test/gtest_pinhole.cpp
+++ b/test/gtest_pinhole.cpp
@@ -5,7 +5,8 @@
* Author: jsola
*/
-#include "core/math/pinhole_tools.h"
+#include "vision/math/pinhole_tools.h"
+
#include <core/utils/utils_gtest.h>
using namespace Eigen;
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 77a7bcfcc..c14d73d41 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -6,9 +6,10 @@
#include "vision/landmark/landmark_HP.h"
#include "vision/capture/capture_image.h"
#include "vision/internal/config.h"
-#include <core/utils/utils_gtest.h>
+
// wolf includes
+#include <core/utils/utils_gtest.h>
#include <core/sensor/sensor_factory.h>
// Vision utils includes
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index babf3e544..3cc902a0c 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -1,16 +1,16 @@
-#include <core/utils/utils_gtest.h>
-
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
-
-#include "vision_utils.h"
#include "vision/processor/processor_tracker_feature_trifocal.h"
-#include "core/processor/processor_odom_3D.h"
#include "vision/capture/capture_image.h"
#include "vision/sensor/sensor_camera.h"
#include "vision/internal/config.h"
+#include <core/utils/utils_gtest.h>
+#include "core/common/wolf.h"
+#include "core/utils/logging.h"
+#include "core/processor/processor_odom_3D.h"
+
+#include "vision_utils/vision_utils.h"
+
using namespace Eigen;
using namespace wolf;
--
GitLab
From 160d8113e98a7f71c5b12591cd303eb029a7bf2c Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 17:56:08 +0200
Subject: [PATCH 26/58] Use prefix 'vision_utils' in includes
---
include/vision/factor/factor_autodiff_trifocal.h | 4 ++--
include/vision/feature/feature_point_image.h | 2 +-
.../vision/processor/processor_tracker_feature_image.h | 8 ++++----
.../processor/processor_tracker_feature_trifocal.h | 10 +++++-----
.../processor/processor_tracker_landmark_image.h | 8 ++++----
src/processor/processor_tracker_feature_image.cpp | 8 ++++----
src/processor/processor_tracker_feature_trifocal.cpp | 10 +++++-----
7 files changed, 25 insertions(+), 25 deletions(-)
diff --git a/include/vision/factor/factor_autodiff_trifocal.h b/include/vision/factor/factor_autodiff_trifocal.h
index 71002f224..4ad34ce40 100644
--- a/include/vision/factor/factor_autodiff_trifocal.h
+++ b/include/vision/factor/factor_autodiff_trifocal.h
@@ -6,8 +6,8 @@
#include "core/factor/factor_autodiff.h"
#include "vision/sensor/sensor_camera.h"
-#include <common_class/trifocaltensor.h>
-#include <vision_utils.h>
+#include <vision_utils/common_class/trifocaltensor.h>
+#include <vision_utils/vision_utils.h>
namespace wolf
{
diff --git a/include/vision/feature/feature_point_image.h b/include/vision/feature/feature_point_image.h
index b4d7d0891..b95839d7f 100644
--- a/include/vision/feature/feature_point_image.h
+++ b/include/vision/feature/feature_point_image.h
@@ -5,7 +5,7 @@
#include "core/feature/feature_base.h"
// Vision Utils includes
-#include <vision_utils.h>
+#include <vision_utils/vision_utils.h>
namespace wolf {
diff --git a/include/vision/processor/processor_tracker_feature_image.h b/include/vision/processor/processor_tracker_feature_image.h
index f65deb9bc..1ee0f78ae 100644
--- a/include/vision/processor/processor_tracker_feature_image.h
+++ b/include/vision/processor/processor_tracker_feature_image.h
@@ -12,10 +12,10 @@
#include "vision/processor/processor_params_image.h"
// vision_utils
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils/detectors/detector_base.h>
+#include <vision_utils/descriptors/descriptor_base.h>
+#include <vision_utils/matchers/matcher_base.h>
+#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
// General includes
#include <cmath>
diff --git a/include/vision/processor/processor_tracker_feature_trifocal.h b/include/vision/processor/processor_tracker_feature_trifocal.h
index 51e109cba..23b25085d 100644
--- a/include/vision/processor/processor_tracker_feature_trifocal.h
+++ b/include/vision/processor/processor_tracker_feature_trifocal.h
@@ -7,11 +7,11 @@
#include "core/utils/params_server.hpp"
// Vision utils
-#include <vision_utils.h>
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/detectors/detector_base.h>
+#include <vision_utils/descriptors/descriptor_base.h>
+#include <vision_utils/matchers/matcher_base.h>
+#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
namespace wolf
{
diff --git a/include/vision/processor/processor_tracker_landmark_image.h b/include/vision/processor/processor_tracker_landmark_image.h
index 2298c9ea9..70273dd20 100644
--- a/include/vision/processor/processor_tracker_landmark_image.h
+++ b/include/vision/processor/processor_tracker_landmark_image.h
@@ -9,10 +9,10 @@
#include "core/processor/processor_tracker_landmark.h"
#include "core/common/wolf.h"
-#include <algorithms/activesearch/alg_activesearch.h>
-#include <descriptors/descriptor_base.h>
-#include <detectors/detector_base.h>
-#include <matchers/matcher_base.h>
+#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils/descriptors/descriptor_base.h>
+#include <vision_utils/detectors/detector_base.h>
+#include <vision_utils/matchers/matcher_base.h>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/mat.inl.hpp>
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index ab7c59b19..88aede08b 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -2,10 +2,10 @@
#include "vision/processor/processor_tracker_feature_image.h"
// Vision utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
+#include <vision_utils/detectors.h>
+#include <vision_utils/descriptors.h>
+#include <vision_utils/matchers.h>
+#include <vision_utils/algorithms.h>
// standard libs
#include <bitset>
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index be96cc33f..ed99f8bb7 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -8,11 +8,11 @@
#include "vision/capture/capture_image.h"
// vision_utils
-#include <vision_utils.h>
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/detectors.h>
+#include <vision_utils/descriptors.h>
+#include <vision_utils/matchers.h>
+#include <vision_utils/algorithms.h>
#include <memory>
--
GitLab
From 340966635c96a9908e21a920f3092da502edc637 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 17:57:03 +0200
Subject: [PATCH 27/58] Organize includes
---
src/yaml/processor_image_yaml.cpp | 4 ++--
src/yaml/processor_tracker_feature_trifocal_yaml.cpp | 5 ++---
2 files changed, 4 insertions(+), 5 deletions(-)
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index 2ab14bfa4..f8cdeeac0 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -5,6 +5,8 @@
* \author: jsola
*/
+#include "vision/processor/processor_params_image.h"
+
// wolf yaml
#include "core/yaml/yaml_conversion.h"
@@ -14,8 +16,6 @@
// yaml-cpp library
#include <yaml-cpp/yaml.h>
-#include "vision/processor/processor_params_image.h"
-
namespace wolf
{
namespace
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index 78bb7ee98..a6619c704 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -5,11 +5,10 @@
* Author: asantamaria
*/
-// wolf yaml
+// wolf
#include "vision/processor/processor_tracker_feature_trifocal.h"
-#include "core/yaml/yaml_conversion.h"
-// wolf
+#include "core/yaml/yaml_conversion.h"
#include "core/common/factory.h"
// yaml-cpp library
--
GitLab
From 114f5ca7767e0fb9aed6a913ef2ad6b0519b6e82 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 17:57:26 +0200
Subject: [PATCH 28/58] Remove file that is already in plugin 'apriltag'
---
...ocessor_tracker_landmark_apriltag_yaml.cpp | 92 -------------------
1 file changed, 92 deletions(-)
delete mode 100644 src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
deleted file mode 100644
index 70a44fc67..000000000
--- a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/**
- * \file processor_tracker_landmark_apriltag_yaml.cpp
- *
- * Created on: Dec 6, 2018
- * \author: jsola
- */
-
-
-// wolf
-#include "base/processor/processor_tracker_landmark_apriltag.h"
-#include "base/yaml/yaml_conversion.h"
-#include "base/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config.IsNull())
- {
- WOLF_ERROR("Invalid YAML file!");
- return nullptr;
- }
- else if (config["processor type"].as<std::string>() == "TRACKER LANDMARK APRILTAG")
- {
- ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
- YAML::Node detector_parameters = config["detector parameters"];
- params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>();
- params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>();
- params->nthreads_ = detector_parameters["nthreads"] .as<int>();
- params->debug_ = detector_parameters["debug"] .as<bool>();
- params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>();
- params->ippe_min_ratio_ = detector_parameters["ippe_min_ratio"] .as<Scalar>();
- params->ippe_max_rep_error_ = detector_parameters["ippe_max_rep_error"] .as<Scalar>();
-
- YAML::Node tag_parameters = config["tag parameters"];
- params->tag_family_ = tag_parameters["tag_family"] .as<std::string>();
- // params->tag_black_border_ = tag_parameters["tag_black_border"] .as<int>();
- params->tag_width_default_ = tag_parameters["tag_width_default"] .as<Scalar>();
-
- // read list of tag widths
- YAML::Node tag_widths = config["tag widths"];
- for (auto pair_id_width : tag_widths)
- {
- int tag_id = pair_id_width.first .as<int>();
- Scalar tag_width = pair_id_width.second .as<Scalar>();
- params->tag_widths_.emplace(tag_id, tag_width);
- }
-
- YAML::Node noise = config["noise"];
- params->std_xy_ = noise["std_xy"] .as<Scalar>();
- params->std_z_ = noise["std_z"] .as<Scalar>();
- params->std_rpy_ = M_TORAD * noise["std_rpy_degrees"] .as<Scalar>();
- params->std_pix_ = noise["std_pix"] .as<Scalar>();
-
- YAML::Node vote = config["vote"];
- params->voting_active = vote["voting active"] .as<bool>();
- params->min_time_vote_ = vote["min_time_vote"] .as<Scalar>();
- params->max_time_vote_ = vote["max_time_vote"] .as<Scalar>();
- params->min_features_for_keyframe = vote["min_features_for_keyframe"] .as<unsigned int>();
- params->max_features_diff_ = vote["max_features_diff"] .as<int>();
- params->nb_vote_for_every_first_ = vote["nb_vote_for_every_first"] .as<int>();
- params->enough_info_necessary_ = vote["enough_info_necessary"] .as<bool>();
-
- params->reestimate_last_frame_ = config["reestimate_last_frame"] .as<bool>();
- params->add_3D_cstr_ = config["add_3D_cstr"] .as<bool>();
-
- return params;
- }
- else
- {
- WOLF_ERROR("Wrong processor type! Should be \"TRACKER LANDMARK APRILTAG\"");
- return nullptr;
- }
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_apriltag = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG", createProcessorParamsLandmarkApriltag);
-const bool WOLF_UNUSED registered_prc_apriltag_wrapper = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG WRAPPER", createProcessorParamsLandmarkApriltag);
-
-} // namespace [unnamed]
-
-} // namespace wolf
--
GitLab
From 2f5a01fcdeb0f306047839665d47b0ff6dd1b66a Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 18:07:31 +0200
Subject: [PATCH 29/58] Export pinhole_tools.h when installing
---
CMakeLists.txt | 1 +
1 file changed, 1 insertion(+)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d00762c84..ff95d8697 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -165,6 +165,7 @@ ENDIF(Ceres_FOUND)
SET(HDRS_COMMON
)
SET(HDRS_MATH
+include/vision/math/pinhole_tools.h
)
SET(HDRS_UTILS
)
--
GitLab
From fdfe144ced097d518363f1a732a5c47811485a83 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 23 May 2019 18:29:42 +0200
Subject: [PATCH 30/58] Fix cmake creating folder 'vision' instead of
'plugin_vision'
---
CMakeLists.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff95d8697..7315df21d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -352,7 +352,7 @@ INSTALL(FILES ${PROJECT_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
- DESTINATION include/iri-algorithms/wolf/${PROJECT_NAME}/${PROJECT_NAME}/internal)
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
# INSTALL(FILES "${CMAKE_BINARY_DIR}/cmake_modules/Findwolf${PLUGIN_NAME}.cmake"
# DESTINATION "lib/cmake/${PLUGIN_NAME}")
--
GitLab
From eadb7a9748b8c3df72fc57c364efc7fd183d814a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 24 May 2019 19:42:13 +0200
Subject: [PATCH 31/58] replace Affine --> Isometry
---
include/vision/factor/factor_AHP.h | 6 +++---
include/vision/factor/factor_autodiff_trifocal.h | 2 +-
src/landmark/landmark_AHP.cpp | 4 ++--
src/landmark/landmark_HP.cpp | 4 ++--
src/processor/processor_tracker_landmark_image.cpp | 4 ++--
5 files changed, 10 insertions(+), 10 deletions(-)
diff --git a/include/vision/factor/factor_AHP.h b/include/vision/factor/factor_AHP.h
index 3ef1b6c8b..448ca1fcb 100644
--- a/include/vision/factor/factor_AHP.h
+++ b/include/vision/factor/factor_AHP.h
@@ -116,7 +116,7 @@ inline void FactorAHP::expectation(const T* const _current_frame_p,
using namespace Eigen;
// All involved transforms typedef
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
// world to anchor robot transform
Map<const Matrix<T, 3, 1> > p_w_r0(_anchor_frame_p);
@@ -144,8 +144,8 @@ inline void FactorAHP::expectation(const T* const _current_frame_p,
// hmg point in current camera frame C1
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg_c0(_lmk_hmg);
- Eigen::Matrix<T, 4, 1> landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Affine)
- * T_W_R1. inverse(Eigen::Affine)
+ Eigen::Matrix<T, 4, 1> landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Isometry)
+ * T_W_R1. inverse(Eigen::Isometry)
* T_W_R0
* T_R0_C0
* landmark_hmg_c0;
diff --git a/include/vision/factor/factor_autodiff_trifocal.h b/include/vision/factor/factor_autodiff_trifocal.h
index 4ad34ce40..31b5d5969 100644
--- a/include/vision/factor/factor_autodiff_trifocal.h
+++ b/include/vision/factor/factor_autodiff_trifocal.h
@@ -271,7 +271,7 @@ inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
{
typedef Translation<T, 3> TranslationType;
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
// All input Transforms
TransformType wHr1 = TranslationType(_wtr1) * _wqr1;
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 28efec608..d8d49a66d 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -44,10 +44,10 @@ Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
Eigen::Vector3s LandmarkAHP::point() const
{
using namespace Eigen;
- Transform<Scalar,3,Affine> T_w_r
+ Transform<Scalar,3,Isometry> T_w_r
= Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
* Quaternions(getAnchorFrame()->getO()->getState().data());
- Transform<Scalar,3,Affine> T_r_c
+ Transform<Scalar,3,Isometry> T_r_c
= Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
* Quaternions(getAnchorSensor()->getO()->getState().data());
Vector4s point_hmg_c = getP()->getState();
diff --git a/src/landmark/landmark_HP.cpp b/src/landmark/landmark_HP.cpp
index a9bf53f4d..33e52b303 100644
--- a/src/landmark/landmark_HP.cpp
+++ b/src/landmark/landmark_HP.cpp
@@ -38,10 +38,10 @@ Eigen::Vector3s LandmarkHP::point() const
using namespace Eigen;
/* TODO: done when creating the landmark
- Transform<Scalar,3,Affine> T_w_r
+ Transform<Scalar,3,Isometry> T_w_r
= Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
* Quaternions(getAnchorFrame()->getO()->getState().data());
- Transform<Scalar,3,Affine> T_r_c
+ Transform<Scalar,3,Isometry> T_r_c
= Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
* Quaternions(getAnchorSensor()->getO()->getState().data());
*/
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index b9ae15084..ec11fb573 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -304,7 +304,7 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
// ALL TRANSFORMS
- Transform<Scalar,3,Eigen::Affine> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
+ Transform<Scalar,3,Eigen::Isometry> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
// world to anchor robot frame
Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getP()->getState()); // sadly we cannot put a Map over a translation
@@ -328,7 +328,7 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
// Transform lmk from c0 to c1 and exit
Vector4s landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
- _point3D_hmg = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
+ _point3D_hmg = T_R1_C1.inverse(Eigen::Isometry) * T_W_R1.inverse(Eigen::Isometry) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
}
Scalar ProcessorTrackerLandmarkImage::match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
--
GitLab
From 222cb3b6365bab3c37415f9246764e530abe3600 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:41:50 +0200
Subject: [PATCH 32/58] Add demo bundle adjustment
---
CMakeLists.txt | 2 +-
demos/CMakeLists.txt | 240 +++----------------------------------------
2 files changed, 15 insertions(+), 227 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7315df21d..db4ea725a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -290,7 +290,7 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
# ENDIF(Ceres_FOUND)
-
+add_subdirectory(demos)
#Build tests
#===============EXAMPLE=========================
IF (GLOG_FOUND)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index beae7b87e..5b7c8ec27 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,232 +1,20 @@
-# Dynamically remove objects from list
-# add_executable(test_list_remove test_list_remove.cpp)
-# Matrix product test
-#ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
+if (OpenCV_FOUND)
+ if (${OpenCV_VERSION_MAJOR} GREATER 1)
+ message("-- [INFO] Found OpenCV support")
+ ADD_DEFINITIONS(-DHAVE_OPENCV_H)
+ SET(USE_CV true)
+ else(${OpenCV_VERSION_MAJOR} GREATER 1)
+ message("[WARN] OpenCV support not installed. Minimum 2.4 version required.")
+ message("[WARN] Current version ${OpenCV_VERSION_MAJOR}")
+ endif(${OpenCV_VERSION_MAJOR} GREATER 1)
+else(OpenCV_FOUND)
+ message("[WARN] OpenCV support not installed. Minimum 2.4 version required.")
+endif(OpenCV_FOUND)
-#ADD_EXECUTABLE(test_eigen_template test_eigen_template.cpp)
-ADD_EXECUTABLE(test_fcn_ptr test_fcn_ptr.cpp)
-ADD_EXECUTABLE(test_kf_callback test_kf_callback.cpp)
-TARGET_LINK_LIBRARIES(test_kf_callback ${PROJECT_NAME})
+ADD_EXECUTABLE(demo_processor_bundle_adjustment demo_processor_bundle_adjustment.cpp)
+TARGET_LINK_LIBRARIES(demo_processor_bundle_adjustment ${PLUGIN_NAME})
-ADD_EXECUTABLE(test_wolf_logging test_wolf_logging.cpp)
-TARGET_LINK_LIBRARIES(test_wolf_logging ${PROJECT_NAME})
-IF(Ceres_FOUND)
- # test_processor_odom_3D
- ADD_EXECUTABLE(test_processor_odom_3D test_processor_odom_3D.cpp)
- TARGET_LINK_LIBRARIES(test_processor_odom_3D ${PROJECT_NAME})
-
-# ADD_EXECUTABLE(test_motion_2d test_motion_2d.cpp)
-# TARGET_LINK_LIBRARIES(test_motion_2d ${PROJECT_NAME})
-ENDIF(Ceres_FOUND)
-
-
-
-
-# State blocks with local parametrizations test
-#ADD_EXECUTABLE(test_state_quaternion test_state_quaternion.cpp)
-#TARGET_LINK_LIBRARIES(test_state_quaternion ${PROJECT_NAME})
-
-# Testing Eigen Permutations
-#ADD_EXECUTABLE(test_permutations solver/test_permutations.cpp)
-#TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME})
-
-# Enable Yaml config files
-IF(YAMLCPP_FOUND)
-# ADD_EXECUTABLE(test_yaml test_yaml.cpp)
-# TARGET_LINK_LIBRARIES(test_yaml ${PROJECT_NAME})
-
-# ADD_EXECUTABLE(test_yaml_conversions test_yaml_conversions.cpp)
-# TARGET_LINK_LIBRARIES(test_yaml_conversions ${PROJECT_NAME})
-
- # SensorFactory classes test
-# ADD_EXECUTABLE(test_wolf_factories test_wolf_factories.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_factories ${PROJECT_NAME})
-
- # Map load and save test
-# ADD_EXECUTABLE(test_map_yaml test_map_yaml.cpp)
-# TARGET_LINK_LIBRARIES(test_map_yaml ${PROJECT_NAME})
-ENDIF(YAMLCPP_FOUND)
-
-IF(Suitesparse_FOUND)
-IF (0) # These cannot compile on MacOSX -- we'll fix it some day.
- # Testing a ccolamd test
- ADD_EXECUTABLE(test_ccolamd solver/test_ccolamd.cpp)
- TARGET_LINK_LIBRARIES(test_ccolamd ${PROJECT_NAME})
-
- # Testing a blocks ccolamd test
- ADD_EXECUTABLE(test_ccolamd_blocks solver/test_ccolamd_blocks.cpp)
- TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME})
-
- # Testing an incremental blocks ccolamd test
- ADD_EXECUTABLE(test_incremental_ccolamd_blocks solver/test_incremental_ccolamd_blocks.cpp)
- TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME})
-
- # Testing an incremental QR with block ccolamd test
- ADD_EXECUTABLE(test_iQR solver/test_iQR.cpp)
- TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME})
-
- # Testing QR solver test tending to wolf
- ADD_EXECUTABLE(test_iQR_wolf solver/test_iQR_wolf.cpp)
- TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME})
-
- # Testing SPQR simple test
- #ADD_EXECUTABLE(test_SPQR solver/test_SPQR.cpp)
- #TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME})
-ENDIF(0)
-
-ENDIF(Suitesparse_FOUND)
-
-# Building and populating the wolf tree
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
-
-# Building and populating the wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_imported_graph test_wolf_imported_graph.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_imported_graph ${PROJECT_NAME})
-
-# Comparing performance of wolf auto_diff and ceres auto_diff
-#ADD_EXECUTABLE(test_wolf_autodiffwrapper test_wolf_autodiffwrapper.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_autodiffwrapper ${PROJECT_NAME})
-
-# Prunning wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_prunning test_wolf_prunning.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_prunning ${PROJECT_NAME})
-
-# Sparsification from wolf tree from .graph file (TORO)
-ADD_EXECUTABLE(test_sparsification test_sparsification.cpp)
-TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
-
-# Comparing analytic and autodiff odometry factors
-#ADD_EXECUTABLE(test_analytic_odom_factor test_analytic_odom_factor.cpp)
-#TARGET_LINK_LIBRARIES(test_analytic_odom_factor ${PROJECT_NAME})
-
-# Vision
-IF(vision_utils_FOUND)
-
- IF(Ceres_FOUND)
- # Testing many things for the 3D image odometry
- ADD_EXECUTABLE(test_image test_image.cpp)
- TARGET_LINK_LIBRARIES(test_image ${PROJECT_NAME})
-
- # Processor Image Landmark test
- ADD_EXECUTABLE(test_processor_tracker_landmark_image test_processor_tracker_landmark_image.cpp)
- TARGET_LINK_LIBRARIES(test_processor_tracker_landmark_image ${PROJECT_NAME})
-
- # Simple AHP test
- 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
- ADD_EXECUTABLE(test_projection_points test_projection_points.cpp)
- TARGET_LINK_LIBRARIES(test_projection_points ${PROJECT_NAME})
-
- # Factor test
- ADD_EXECUTABLE(test_factor_AHP test_factor_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_factor_AHP ${PROJECT_NAME})
-
- # ORB tracker test
- ADD_EXECUTABLE(test_tracker_ORB test_tracker_ORB.cpp)
- TARGET_LINK_LIBRARIES(test_tracker_ORB ${PROJECT_NAME})
-
- # Trifocal optimization test
- ADD_EXECUTABLE(test_trifocal_optimization test_trifocal_optimization.cpp)
- TARGET_LINK_LIBRARIES(test_trifocal_optimization ${PROJECT_NAME})
-
-ENDIF(vision_utils_FOUND)
-
-# Processor Tracker Feature test
-ADD_EXECUTABLE(test_processor_tracker_feature test_processor_tracker_feature.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_feature ${PROJECT_NAME})
-
-# Processor Tracker Landmark test
-ADD_EXECUTABLE(test_processor_tracker_landmark test_processor_tracker_landmark.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_landmark ${PROJECT_NAME})
-
-# Processor IMU test
-ADD_EXECUTABLE(test_processor_imu test_processor_imu.cpp)
-TARGET_LINK_LIBRARIES(test_processor_imu ${PROJECT_NAME})
-
-# Processor Diff-Drive test
-ADD_EXECUTABLE(test_processor_diff_drive test_diff_drive.cpp)
-TARGET_LINK_LIBRARIES(test_processor_diff_drive ${PROJECT_NAME})
-
-# MPU6050 IMU test
-#ADD_EXECUTABLE(test_mpu test_mpu.cpp)
-#TARGET_LINK_LIBRARIES(test_mpu ${PROJECT_NAME})
-
-# Processor IMU - Jacobian checking test
-#ADD_EXECUTABLE(test_processor_imu_jacobians test_processor_imu_jacobians.cpp)
-#TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME})
-
-# Test IMU using printed Dock
-#ADD_EXECUTABLE(test_imuDock test_imuDock.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock ${PROJECT_NAME})
-
-# Test IMU with auto KF generation (Humanoids 20017)
-#ADD_EXECUTABLE(test_imuDock_autoKFs test_imuDock_autoKFs.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock_autoKFs ${PROJECT_NAME})
-
-# FactorIMU - factors test
-#ADD_EXECUTABLE(test_imu_constrained0 test_imu_constrained0.cpp)
-#TARGET_LINK_LIBRARIES(test_imu_constrained0 ${PROJECT_NAME})
-
-# IMU - Offline plateform test
-#ADD_EXECUTABLE(test_imuPlateform_Offline test_imuPlateform_Offline.cpp)
-#TARGET_LINK_LIBRARIES(test_imuPlateform_Offline ${PROJECT_NAME})
-
-# IMU - factorIMU
-#ADD_EXECUTABLE(test_factor_imu test_factor_imu.cpp)
-#TARGET_LINK_LIBRARIES(test_factor_imu ${PROJECT_NAME})
-
-# IF (laser_scan_utils_FOUND)
-# ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
-# TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
-# #ENDIF (laser_scan_utils_FOUND)
-
-# IF(faramotics_FOUND)
-# IF (laser_scan_utils_FOUND)
-# ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_2_lasers
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
-# TARGET_LINK_LIBRARIES(test_2_lasers_offline
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
-# TARGET_LINK_LIBRARIES(test_faramotics_simulation
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# # ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
-# # TARGET_LINK_LIBRARIES(test_autodiff
-# # ${pose_state_time_LIBRARIES}
-# # ${faramotics_LIBRARIES}
-# # ${PROJECT_NAME})
-# # IF(Suitesparse_FOUND)
-# # ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
-# # TARGET_LINK_LIBRARIES(test_iQR_wolf2
-# # ${pose_state_time_LIBRARIES}
-# # ${faramotics_LIBRARIES}
-# # ${PROJECT_NAME})
-# # ENDIF(Suitesparse_FOUND)
-# # ENDIF (laser_scan_utils_FOUND)
-# # ENDIF(faramotics_FOUND)
--
GitLab
From ef211f7013a8551399c449ddf53b506ae1ad62b7 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:43:31 +0200
Subject: [PATCH 33/58] params for demo bundle adjustment
---
demos/calib_logitech_webcam_params.yaml | 20 ++++++++++++++++++++
demos/processor_bundle_adjustment.yaml | 7 ++++---
2 files changed, 24 insertions(+), 3 deletions(-)
create mode 100644 demos/calib_logitech_webcam_params.yaml
diff --git a/demos/calib_logitech_webcam_params.yaml b/demos/calib_logitech_webcam_params.yaml
new file mode 100644
index 000000000..1285f904c
--- /dev/null
+++ b/demos/calib_logitech_webcam_params.yaml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: narrow_stereo
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [942.816482, 0.000000, 334.175643, 0.000000, 897.287666, 309.904004, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.159958, -0.578834, 0.000000, 0.000000, 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: [953.235413, 0.000000, 334.175675, 0.000000, 0.000000, 907.202332, 309.904238, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/processor_bundle_adjustment.yaml b/demos/processor_bundle_adjustment.yaml
index 788373081..52242f47c 100644
--- a/demos/processor_bundle_adjustment.yaml
+++ b/demos/processor_bundle_adjustment.yaml
@@ -1,11 +1,12 @@
-processor type: "TRACKER FEATURE TRIFOCAL"
-processor name: "tracker feature trifocal example"
+processor type: "TRACKER BUNDLE ADJUSTMENT"
+processor name: "tracker bundle adjustment example"
vision_utils:
- YAML file params: processor_tracker_feature_trifocal_vision_utils.yaml
+ YAML file params: processor_bundle_adjustment_vision_utils.yaml
algorithm:
time tolerance: 0.005
+ delete ambiguities: true
voting active: false
minimum features for keyframe: 40
maximum new features: 100
--
GitLab
From ebbe4c1de6f91be29b1812b4b9f3df5e286503f6 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:44:04 +0200
Subject: [PATCH 34/58] Add demo bundle adjustment
---
demos/demo_processor_bundle_adjustment.cpp | 254 +++++++++++++++++++++
1 file changed, 254 insertions(+)
create mode 100644 demos/demo_processor_bundle_adjustment.cpp
diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp
new file mode 100644
index 000000000..6ef81f47e
--- /dev/null
+++ b/demos/demo_processor_bundle_adjustment.cpp
@@ -0,0 +1,254 @@
+//std
+#include <iostream>
+
+#include "vision/processor/processor_bundle_adjustment.h"
+
+//Wolf
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
+#include <core/state_block/state_block.h>
+#include "vision/sensor/sensor_camera.h"
+#include "vision/capture/capture_image.h"
+#include <core/ceres_wrapper/ceres_manager.h>
+#include "vision/landmark/landmark_HP.h"
+#include "vision/internal/config.h"
+
+
+// Vision utils includes
+#include <vision_utils.h>
+#include <sensors.h>
+#include <common_class/buffer.h>
+#include <common_class/frame.h>
+
+////Mvbluefox includes
+//#include <iri/mvbluefox3/mvbluefox3.h>
+//#include <unistd.h>
+//#include <limits.h>
+//#include <sstream>
+//
+//typedef bool( *SUPPORTED_DEVICE_CHECK )( const mvIMPACT::acquire::Device* const );
+//
+//// mvIMPACT::acquire::DeviceManager devMgr_; // Device Manager.
+//
+//
+//std::string GetSerialFromUser(void)
+//{
+// mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
+// return getDeviceFromUserInput(devMgr)->serial.read();
+//}
+//
+//std::string AvailableDevices(SUPPORTED_DEVICE_CHECK pSupportedDeviceCheckFn)
+//{
+// mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
+// std::ostringstream devices;
+// devices << "Available device(s): ";
+// const unsigned int devCnt = devMgr.deviceCount();
+// if (devCnt == 0)
+// {
+// devices << "0.";
+// }
+// else
+// {
+// for( unsigned int i = 0; i < devCnt; i++ )
+// {
+// Device* pDev = devMgr[i];
+// if( pDev )
+// {
+// if( !pSupportedDeviceCheckFn || pSupportedDeviceCheckFn( pDev ) )
+// {
+// devices << " \n - " << pDev->product.read() << ": " << pDev->serial.read();
+// }
+// }
+// }
+// }
+// return devices.str();
+//}
+//
+//std::string GetPath()
+//{
+// char result[ PATH_MAX ];
+// ssize_t count = readlink( "/proc/self/exe", result, PATH_MAX );
+// return std::string( result, (count > 0) ? count : 0 );
+//}
+
+using Eigen::Vector3s;
+using Eigen::Vector4s;
+using Eigen::Vector6s;
+using Eigen::Vector7s;
+
+using namespace wolf;
+
+std::string wolf_vision_root = _WOLF_VISION_ROOT_DIR;
+
+int main(int argc, char** argv)
+{
+ std::cout << std::endl << "==================== processor bundle adjustment test ======================" << std::endl;
+
+// std::cout << "______________________________Get image from driver_________________________" << std::endl;
+// // Sensor or sensor recording
+//
+// int num_cams = 1;
+//
+// // Get own path
+// std::string argv_str(argv[0]);
+//
+// std::cout << argv[0] << "\n";
+//
+// std::string pbase = argv_str.substr(0, argv_str.find_last_of("/"));
+// std::string paramspath = pbase + "/../demos/demo_processor_bundle_adjustment_params.xml";
+//
+// std::cout << paramspath << "\n";
+//
+// std::cout << std::endl;
+// std::cout << "******************************" << std::endl;
+// std::cout << "* ---- mvBlueFOX3 ---- *" << std::endl;
+// std::cout << "******************************" << std::endl;
+// std::cout << std::endl;
+// std::cout << "Please, choose the device to test from the following list:" << std::endl;
+// std::cout << std::endl;
+//
+// std::vector<std::string> serial;
+// serial.resize(num_cams);
+// serial[0] = GetSerialFromUser();
+//
+// std::vector<std::string> window_name;
+// window_name.resize(num_cams);
+// window_name[0] = serial[0];
+//
+//
+// mvIMPACT::acquire::DeviceManager devMgr_; // Device Manager.
+// std::vector<CMvbluefox3::CMvbluefox3*> cam_ptr; // Device.
+//
+// try {
+// cam_ptr.resize(num_cams);
+// for (int ii = 0; ii < num_cams; ++ii)
+// {
+// cam_ptr[ii] = NULL;
+//
+// if( !serial[ii].empty() )
+// {
+// std::cout << "Trying to open device with serial: " << serial[ii] << std::endl;
+//
+// if (devMgr_.getDeviceBySerial(serial[ii]) == 0)
+// std::cout << "No device found! Unable to continue. " << std::endl;
+// else
+// cam_ptr[ii] = new CMvbluefox3::CMvbluefox3(devMgr_.getDeviceBySerial(serial[ii]),paramspath);
+// }
+// else
+// std::cout << "Device cannot be found because serial number is not specified." << std::endl;
+// }
+//
+//// std::cout << "Acquiring images." << std::endl << ">WARNING: depending on OpenCV compilation flags, visualization might experience hard delays (e.g. if \"init done\" appears in the following line)" << std::endl;
+// for (int ii = 0; ii < num_cams; ++ii)
+// cv::namedWindow( window_name[ii], cv::WINDOW_NORMAL );
+//
+// while (true)
+// {
+// for (int ii = 0; ii < num_cams; ++ii)
+// {
+// cv::Mat image;
+// cam_ptr[ii]->GetImageCV(image);
+// cv::imshow( window_name[ii], image );
+// }
+// if(cv::waitKey(30) >= 0) break;
+// }
+//
+// cv::destroyAllWindows();
+//
+// std::cout << "[Camera test]: Finished successfully." << std::endl;
+// }catch (CMvbluefox3::CmvBlueFOX3Exception& e) {
+// std::cout << e.what() << std::endl;
+// }
+//
+// for (int ii = 0; ii < num_cams; ++ii)
+// {
+// if (cam_ptr[ii] != NULL)
+// {
+// cam_ptr[ii] = NULL;
+// delete [] cam_ptr[ii];
+// }
+// }
+//
+
+ vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
+ if (sen_ptr==NULL)
+ return 0;
+
+ unsigned int buffer_size = 10;
+ vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
+ frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
+
+ unsigned int img_width = frame_buff.back()->getImage().cols;
+ unsigned int img_height = frame_buff.back()->getImage().rows;
+ std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
+
+// ===========================================================================
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
+ ceres::Solver::Options options;
+ options.max_num_iterations = 100;
+ options.function_tolerance = 1e-4;
+ CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, options);
+
+ // Install camera
+// IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+// intr->pinhole_model_raw = Eigen::Vector4s(320,240,320,320);
+// intr->width = img_width;
+// intr->height = img_height;
+// auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+ auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), wolf_vision_root + "/demos/calib_logitech_webcam_params.yaml");
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ camera->setImgWidth(img_width);
+ camera->setImgHeight(img_height);
+
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 200;
+ params->min_features_for_keyframe = 50;
+ params->time_tolerance = 0.01;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+
+// ==============================================================================
+
+ TimeStamp t = 0;
+ unsigned int number_of_KFs = 0;
+ // main loop
+ Scalar dt = 0.04;
+ for(int frame_count = 0; frame_count<10000; ++frame_count)
+ {
+ t += dt;
+
+ // Image ---------------------------------------------
+ frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
+ CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
+ /* process */
+ camera->process(image);
+
+ // solve only when new KFs are added
+ if (problem->getTrajectory()->getFrameList().size() > number_of_KFs)
+ {
+ number_of_KFs = problem->getTrajectory()->getFrameList().size();
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ if (number_of_KFs > 5)
+ break;
+
+ }
+
+ }
+
+ problem->print(1,0,1,0);
+
+ return 0;
+}
+
--
GitLab
From 740f734365a2321798929962ad4b7f6ada1fe547 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:44:37 +0200
Subject: [PATCH 35/58] Suggest (commented) alternative correction model. Keep
original one.
---
include/vision/math/pinhole_tools.h | 39 +++++++++++++++++++++++++++++
1 file changed, 39 insertions(+)
diff --git a/include/vision/math/pinhole_tools.h b/include/vision/math/pinhole_tools.h
index ef226651e..85ee2062a 100644
--- a/include/vision/math/pinhole_tools.h
+++ b/include/vision/math/pinhole_tools.h
@@ -761,6 +761,45 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
c = iRd * (rc - rd);
}
+
+// //This method follows the one in: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4934233/
+// SizeEigen size = c.size();
+// if (size != 0 && size <=9) //Only implemented to compute a maximum of 9 distortion correction parameters
+// {
+// c(0) = -d(0);
+// if (d.size() >= 2)
+// {
+// c(1) = 3 * pow(d(0),2) - d(1);
+// }
+//
+// if (d.size() >= 3)
+// {
+// c(2) = -12 * pow(d(0), 3) + 8 * d(0) * d(1) - d(2);
+//
+//// c(3) = 55 * pow(d(0), 4) - 55 * pow(d(0), 2) * d(1) + 5 * pow(d(1), 2) + 10 * d(0) * d(2) - d(3);
+////
+//// c(4) = -273 * pow(d(0), 5) + 364 * pow(d(0), 3) * d(1) - 78 * d(0) * pow(d(1), 2) - 78 * pow(d(0), 2) * d(2) + 12 * d(1) * d(2) + 12 * d(0) * d(3);
+////
+//// c(5) = 1428 * pow(d(0), 6) - 2380 * pow(d(0), 4) * d(1) + 840 * pow(d(0), 2) * pow(d(1), 2) - 35 * pow(d(1), 3) + 560 * pow(d(0), 3) * d(2)
+//// - 210 * d(0) * d(1) * d(2) + 7 * pow(d(2), 2) - 105 * pow(d(0), 2) * d(3) + 14 * d(1) * d(3);
+////
+//// c(6) = -7752 * pow(d(0), 7) + 15504 * pow(d(0), 5) * d(1) - 7752 * pow(d(0), 3) * pow(d(1), 2) + 816 * d(0) * pow(d(1), 3) + 2448 * pow(d(0), 2) * d(1) * d(2)
+//// - 136 * pow(d(1), 2) * d(2) - 136 * d(0) * pow(d(2), 2) + 816 * pow(d(0), 3) * d(3) - 272 * d(0) * d(1) * d(3) + 16 * d(2) * d(3);
+////
+//// c(7) = 43263 * pow(d(0), 8) - 100947 * pow(d(0), 6) * d(1) + 65835 * pow(d(0), 4) * pow(d(1), 2) - 11970 * pow(d(0), 2) * pow(d(1), 3) + 285 * pow(d(1), 4)
+//// + 26334 * pow(d(0), 5) * d(2) - 23940 * pow(d(0), 3) * d(1) * d(2) + 3420 * d(0) * pow(d(1), 2) * d(2) + 1710 * pow(d(0), 2) * pow(d(2), 2)
+//// - 171 * d(1) * pow(d(2), 2) - 5985 * pow(d(0), 4) * d(3) + 3420 * pow(d(0), 2) * d(1) * d(3) - 171 * pow(d(1), 2) * d(3) - 342 * d(0) * d(2) * d(3)
+//// + 9 * pow(d(3), 2);
+////
+//// c(8) = -246675 * pow(d(0), 9) + 657800 * pow(d(0), 7) * d(1) - 531300 * pow(d(0), 5) * pow(d(1), 2) + 141680 * pow(d(0), 3) * pow(d(1), 3)
+//// - 8855 * d(0) * pow(d(1), 4) - 177100 * pow(d(0), 6) * d(2) + 212520 * pow(d(0), 4) * d(1) * d(2) - 53130 * pow(d(0), 2) * pow(d(1), 2) * d(2)
+//// + 1540 * pow(d(1), 3) * d(2) - 17710 * pow(d(0), 3) * pow(d(2), 2) + 4620 * d(0) * d(1) * pow(d(2), 2) - 70 * pow(d(2), 3)
+//// + 42504 * pow(d(0), 5) * d(3) - 35420 * pow(d(0), 3) * d(1) * d(3) + 4620 * d(0) * pow(d(1), 2) * d(3) + 4620 * pow(d(0), 2) * d(2) * d(3)
+//// - 420 * d(1) * d(2) * d(3) - 210 * d(0) * pow(d(3), 2);
+// }
+// c = c.head(size);
+// }
+
}
} // namespace pinhole
--
GitLab
From 9bc84c485a33c9c69713b883ac0c216624093c71 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:47:14 +0200
Subject: [PATCH 36/58] Improve processor bundle adj.
---
.../processor/processor_bundle_adjustment.h | 9 ++
src/processor/processor_bundle_adjustment.cpp | 106 ++++++++++++------
2 files changed, 80 insertions(+), 35 deletions(-)
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index 8c55cdb5b..c13f4cad3 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -12,6 +12,8 @@
#include "core/processor/processor_tracker_feature.h"
#include "vision/capture/capture_image.h"
#include "vision/landmark/landmark_HP.h"
+#include "vision/math/pinhole_tools.h"
+#include "vision/sensor/sensor_camera.h"
//vision utils includes
#include "vision_utils/vision_utils.h"
@@ -144,6 +146,8 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
*/
virtual void establishFactors() override;
+ void setParams(const ProcessorParamsBundleAdjustmentPtr _params);
+
public:
/// @brief Factory method
static ProcessorBasePtr create(const std::string& _unique_name,
@@ -162,6 +166,11 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
cv::Mat getImageDebug();
};
+inline cv::Mat ProcessorBundleAdjustment::getImageDebug()
+{
+ return image_debug_;
+}
+
} //namespace wolf
#endif /* INCLUDE_BASE_PROCESSOR_PROCESSOR_BUNDLE_ADJUSTMENT_H_ */
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 0c4b8a0c4..cd5ce5347 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -122,6 +122,51 @@ void ProcessorBundleAdjustment::postProcess()
// DEBUG
image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks);
+// typedef std::map<size_t, LandmarkBasePtr>::iterator iter;
+// for(iter it = lmk_track_map_.begin(); it != lmk_track_map_.end(); ++it)
+// {
+// Vector3s point3D = std::static_pointer_cast<LandmarkHP>(it->second)->point();
+// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
+// Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D);
+// cv::Point point = cv::Point(point2D(0), point2D(1));
+// cv::circle(image_debug_, point, 1, cv::Scalar(0,0,255) , 1 , 8);
+// }
+
+// for (auto const & feat_base : last_ptr_->getFeatureList())
+// {
+// FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
+// unsigned int track_id = feat->trackId();
+// Vector3s point3D = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
+// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
+// Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D);
+// cv::Point point = cv::Point(point2D(0), point2D(1));
+// cv::circle(image_debug_, point, 1, cv::Scalar(0,0,255) , 1 , 8);
+// }
+
+ list<FeatureBasePtr> snapshot = track_matrix_.snapshotAsList(last_ptr_);
+ for (auto const & feat_base : snapshot)
+ {
+ FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ unsigned int track_id = feat->trackId();
+ if (lmk_track_map_.count(track_id))
+ {
+ Vector3s point3D = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(feat_base->getFrame()->getP()->getState())
+ * Quaternions(feat_base->getFrame()->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(feat_base->getCapture()->getSensorP()->getState())
+ * Quaternions(feat_base->getCapture()->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 3, 1> point3D_c = T_r_c.inverse()
+ * T_w_r.inverse()
+ * point3D;
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
+ Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D_c);
+ cv::Point point = cv::Point(point2D(0), point2D(1));
+ cv::circle(image_debug_, point, 3, cv::Scalar(0,0,255) , 1 , 8);
+ }
+ }
+
cv::imshow("DEBUG VIEW", image_debug_);
cv::waitKey(1);
}
@@ -156,7 +201,8 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
}
}
}
- return _feature_correspondences.size();
+// return _feature_correspondences.size();
+ return _features_incoming_out.size();
}
@@ -253,14 +299,18 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
Eigen::Vector2s point2D = _feature_ptr->getMeasurement();
Eigen::Vector3s point3D;
- point3D = pinhole::backprojectPoint(getSensor()->getIntrinsic()->getState(),(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(), point2D);
- point3D.normalize();
+ point3D = pinhole::backprojectPoint(
+ getSensor()->getIntrinsic()->getState(),
+ (std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
+ point2D);
+
//Scalar distance = params_bundle_adjustment_->distance; // arbitrary value
Scalar distance = 1;
- Eigen::Vector4s vec_homogeneous;
- vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
+ Eigen::Vector4s vec_homogeneous_c;
+ vec_homogeneous_c = {point3D(0),point3D(1),point3D(2),point3D.norm()/distance};
+ vec_homogeneous_c.normalize();
//TODO: lmk from camera to world coordinate frame.
Transform<Scalar,3,Isometry> T_w_r
@@ -271,7 +321,7 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
* Quaternions(_feature_ptr->getCapture()->getSensorO()->getState().data());
Eigen::Matrix<Scalar, 4, 1> vec_homogeneous_w = T_w_r
* T_r_c
- * vec_homogeneous;
+ * vec_homogeneous_c;
// std::cout << "Point2D: " << point2D.transpose() << std::endl;
@@ -280,7 +330,7 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
// std::cout << "Hmg_w: " << vec_homogeneous_w.transpose() << std::endl;
//vec_homogeneous_w = vec_homogeneous;
- LandmarkHPPtr lmk_hp_ptr = std::make_shared<LandmarkHP>(vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
+ LandmarkBasePtr lmk_hp_ptr = LandmarkBase::emplace<LandmarkHP>(getProblem()->getMap(), vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
_feature_ptr->setLandmarkId(lmk_hp_ptr->id());
return lmk_hp_ptr;
}
@@ -288,6 +338,10 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
void ProcessorBundleAdjustment::establishFactors()
{
+ if (origin_ptr_ == last_ptr_)
+ return;
+
+
TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, last_ptr_);
for (auto const & pair_trkid_pair : matches_origin_inc)
@@ -296,12 +350,8 @@ void ProcessorBundleAdjustment::establishFactors()
FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_last = pair_trkid_pair.second.second;
-// assert(feature_origin!=nullptr && "null feature origin");
-// assert(feature_last!=nullptr && "null feature last");
-// assert(feature_origin->getCapture()!=nullptr);
-// assert(feature_last->getCapture()!=nullptr);
-// assert(feature_last->getCapture()->getFrame()!=nullptr && feature_last->getCapture()->getFrame()->isKey());
-// assert(feature_origin->getCapture()->getFrame()!=nullptr && feature_origin->getCapture()->getFrame()->isKey());
+ assert(feature_origin!=feature_last && "feature: origin equal last");
+ assert(feature_origin->getCapture()!=feature_last->getCapture() && "capture: origin equal last");
if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
{
@@ -309,7 +359,6 @@ void ProcessorBundleAdjustment::establishFactors()
LandmarkBasePtr lmk = createLandmark(feature_origin);
assert(lmk!=nullptr);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
- getProblem()->addLandmark(lmk_hp);
//add Landmark to map: map[track_id] = landmarkptr
lmk_track_map_[trkid] = lmk;
@@ -318,20 +367,8 @@ void ProcessorBundleAdjustment::establishFactors()
// assert(feature_origin->getCapture()->getSensorP() != nullptr);
// assert(feature_origin->getCapture()->getSensorO() != nullptr);
// assert(lmk->getP() != nullptr);
- FactorPixelHPPtr fac_ptr_origin = std::make_shared<FactorPixelHP>(feature_origin, lmk_hp, shared_from_this());
- if (fac_ptr_origin != nullptr) // factor links
- {
- feature_origin->addFactor(fac_ptr_origin);
- fac_ptr_origin->link(feature_origin);
- lmk->addConstrainedBy(fac_ptr_origin);
- }
- FactorPixelHPPtr fac_ptr_last = std::make_shared<FactorPixelHP>(feature_last, lmk_hp, shared_from_this());
- if (fac_ptr_last != nullptr) // factor links
- {
- feature_last->addFactor(fac_ptr_last);
- fac_ptr_last->link(feature_last);
- lmk->addConstrainedBy(fac_ptr_last);
- }
+ FactorBase::emplace<FactorPixelHP>(feature_origin, feature_origin, lmk_hp, shared_from_this());
+ FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
}
else
{
@@ -339,17 +376,16 @@ void ProcessorBundleAdjustment::establishFactors()
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
//Create factor
- FactorPixelHPPtr fac_ptr_last = std::make_shared<FactorPixelHP>(feature_last, lmk_hp, shared_from_this());
- if (fac_ptr_last != nullptr) // factor links
- {
- feature_last->addFactor(fac_ptr_last);
- fac_ptr_last->link(feature_last);
- lmk->addConstrainedBy(fac_ptr_last);
- }
+ FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
}
}
}
+void ProcessorBundleAdjustment::setParams(const ProcessorParamsBundleAdjustmentPtr _params)
+{
+ params_bundle_adjustment_ = _params;
+}
+
ProcessorBasePtr ProcessorBundleAdjustment::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr)
--
GitLab
From 3c7d08cecf1947fb76d989498b69e6e940c11d9e Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:47:43 +0200
Subject: [PATCH 37/58] fix int type
---
src/yaml/processor_tracker_feature_trifocal_yaml.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index a6619c704..9257e0373 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -46,7 +46,7 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
params->voting_active = algorithm["voting active"] .as<bool>();
params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
- params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
+ params->max_new_features = algorithm["maximum new features"] .as<int>();
params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
params->n_cells_v = algorithm["grid vert cells"] .as<int>();
params->min_response_new_feature = algorithm["min response new features"] .as<int>();
--
GitLab
From e00aa392aef5c4f2602a686f74165fa143a06112 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:48:10 +0200
Subject: [PATCH 38/58] use correction model 1 unit larger than distortion
model
---
src/sensor/sensor_camera.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 39386d4d2..44b7fa6b0 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -13,7 +13,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsC
img_width_(_intrinsics.width), //
img_height_(_intrinsics.height), //
distortion_(_intrinsics.distortion), //
- correction_(distortion_.size()==0 ? 0 : distortion_.size()), // make correction vector slightly larger in size than the distortion vector
+ correction_(distortion_.size()==0 ? 0 : distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
using_raw_(true)
--
GitLab
From a6dcb6967b077f3748916bbbe9a2a9922ff24d6e Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:48:39 +0200
Subject: [PATCH 39/58] Add test for complete BA
---
test/gtest_factor_pixelHP.cpp | 261 +++++++++++++++++++++++++++++++---
1 file changed, 243 insertions(+), 18 deletions(-)
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 59647ad9f..318226812 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -20,7 +20,7 @@
using namespace wolf;
using namespace Eigen;
-std::string wolf_root = _WOLF_VISION_ROOT_DIR;
+std::string wolf_vision_root = _WOLF_VISION_ROOT_DIR;
class FactorPixelHPTest : public testing::Test{
@@ -35,23 +35,26 @@ class FactorPixelHPTest : public testing::Test{
ProblemPtr problem;
CeresManagerPtr ceres_manager;
+
SensorCameraPtr camera;
ProcessorBundleAdjustmentPtr proc_bundle_adjustment;
SensorBasePtr S;
FrameBasePtr F1, F2, F3;
CaptureImagePtr I1, I2, I3;
- FeaturePointImagePtr f1, f2, f3;
+ FeaturePointImagePtr f11, f21, f31;
FeaturePointImagePtr f12, f13, f14;
+ FeaturePointImagePtr f22, f23, f24;
+ FeaturePointImagePtr f32, f33, f34;
LandmarkHPPtr L1; //only one landmark L1 is initialized
LandmarkHPPtr L2;
LandmarkHPPtr L3;
LandmarkHPPtr L4;
- FactorPixelHPPtr c1, c12, c13, c14;
- FactorPixelHPPtr c2;
- FactorPixelHPPtr c3;
+ FactorPixelHPPtr c11, c12, c13, c14;
+ FactorPixelHPPtr c21, c22, c23, c24;
+ FactorPixelHPPtr c31, c32, c33, c34;
Scalar pixel_noise_std;
@@ -146,8 +149,11 @@ class FactorPixelHPTest : public testing::Test{
// Build problem
problem = Problem::create("PO", 3);
ceres::Solver::Options options;
+ options.function_tolerance = 1e-6;
+ options.max_num_iterations = 200;
ceres_manager = std::make_shared<CeresManager>(problem, options);
+
// Install sensor and processor
IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
intr->pinhole_model_raw = Eigen::Vector4s(320,240,320,320);
@@ -179,15 +185,15 @@ class FactorPixelHPTest : public testing::Test{
F1 = problem->emplaceFrame(KEY, pose1, 1.0);
I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
- f1 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp, 0, des, pix_cov)); // pixel at origin
+ f11 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp, 0, des, pix_cov)); // pixel at origin
F2 = problem->emplaceFrame(KEY, pose2, 2.0);
I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1))));
- f2 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp, 0, des, pix_cov)); // pixel at origin
+ f21 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp, 0, des, pix_cov)); // pixel at origin
F3 = problem->emplaceFrame(KEY, pose3, 3.0);
I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
- f3 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp, 0, des, pix_cov)); // pixel at origin
+ f31 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp, 0, des, pix_cov)); // pixel at origin
//create Landmark -- with f1, f2 or f3
//LandmarkBasePtr lmk = proc_bundle_adjustment->createLandmark(f1);
@@ -196,9 +202,9 @@ class FactorPixelHPTest : public testing::Test{
// lmk->link(problem->getMap());
// factors
- c1 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f1, f1, L1, nullptr /*proc*/));
- c2 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f2, f2, L1, nullptr /*proc*/));
- c3 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f3, f3, L1, nullptr /*proc*/));
+ c11 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f11, f11, L1, nullptr /*proc*/));
+ c21 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f21, f21, L1, nullptr /*proc*/));
+ c31 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f31, f31, L1, nullptr /*proc*/));
}
};
@@ -218,7 +224,7 @@ TEST(ProcessorFactorPixelHP, testZeroResidual)
// Install processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -341,9 +347,9 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition2ObservableDoF)
// Frame must be in the X axis, so Y=0 and Z=0
ASSERT_MATRIX_APPROX(F1->getP()->getState().tail(2), orig.tail(2), 1e-6);
- Eigen::VectorXs expect = c1->expectation();
- ASSERT_FLOAT_EQ(expect(0,0),f1->getMeasurement()(0,0));
- ASSERT_FLOAT_EQ(expect(1,0),f1->getMeasurement()(1,0));
+ Eigen::VectorXs expect = c11->expectation();
+ ASSERT_FLOAT_EQ(expect(0,0),f11->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),f11->getMeasurement()(1,0));
}
@@ -430,15 +436,234 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
// This test checks 3 DoF (3DoF are observable).
ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
- Eigen::VectorXs expect = c1->expectation();
- ASSERT_FLOAT_EQ(expect(0,0),f1->getMeasurement()(0,0));
- ASSERT_FLOAT_EQ(expect(1,0),f1->getMeasurement()(1,0));
+ Eigen::VectorXs expect = c11->expectation();
+ ASSERT_FLOAT_EQ(expect(0,0),f11->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),f11->getMeasurement()(1,0));
+}
+
+
+TEST_F(FactorPixelHPTest, testSolveBundleAdjustment)
+{
+ //landmark homogeneous coordinates in world reference
+ lmkHP2 << 0.5, 0.5, 0.5, 1;
+ lmkHP3 << -0.5, 0.75, 0.5, 1;
+ lmkHP4 << 0.5, 0, -0.5, 1;
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(F1->getP()->getState())
+ * Quaternions(F1->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(I1->getSensorP()->getState())
+ * Quaternions(I1->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c = lmkHP2_c.head<3>()/lmkHP2_c(3);
+ Vector3s lmk_point_3_c = lmkHP3_c.head<3>()/lmkHP3_c(3);
+ Vector3s lmk_point_4_c = lmkHP4_c.head<3>()/lmkHP4_c(3);
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r2
+ = Translation<Scalar,3>(F2->getP()->getState())
+ * Quaternions(F2->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c2
+ = Translation<Scalar,3>(I2->getSensorP()->getState())
+ * Quaternions(I2->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c2 = T_r_c2.inverse() * T_w_r2.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c2 = T_r_c2.inverse() * T_w_r2.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c2 = T_r_c2.inverse() * T_w_r2.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c2 = lmkHP2_c2.head<3>()/lmkHP2_c2(3);
+ Vector3s lmk_point_3_c2 = lmkHP3_c2.head<3>()/lmkHP3_c2(3);
+ Vector3s lmk_point_4_c2 = lmkHP4_c2.head<3>()/lmkHP4_c2(3);
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r3
+ = Translation<Scalar,3>(F3->getP()->getState())
+ * Quaternions(F3->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c3
+ = Translation<Scalar,3>(I3->getSensorP()->getState())
+ * Quaternions(I3->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c3 = T_r_c3.inverse() * T_w_r3.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c3 = T_r_c3.inverse() * T_w_r3.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c3 = T_r_c3.inverse() * T_w_r3.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c3 = lmkHP2_c3.head<3>()/lmkHP2_c3(3);
+ Vector3s lmk_point_3_c3 = lmkHP3_c3.head<3>()/lmkHP3_c3(3);
+ Vector3s lmk_point_4_c3 = lmkHP4_c3.head<3>()/lmkHP4_c3(3);
+
+ //points projected to image plane
+ Vector2s point2 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c);
+ Vector2s point3 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c);
+ Vector2s point4 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c);
+ cv::KeyPoint kp12 = cv::KeyPoint(cv::Point2f(point2(0),point2(1)), 32.0f);
+ cv::KeyPoint kp13 = cv::KeyPoint(cv::Point2f(point3(0),point3(1)), 32.0f);
+ cv::KeyPoint kp14 = cv::KeyPoint(cv::Point2f(point4(0),point4(1)), 32.0f);
+
+ Vector2s point22 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c2);
+ Vector2s point23 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c2);
+ Vector2s point24 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c2);
+ cv::KeyPoint kp22 = cv::KeyPoint(cv::Point2f(point22(0),point22(1)), 32.0f);
+ cv::KeyPoint kp23 = cv::KeyPoint(cv::Point2f(point23(0),point23(1)), 32.0f);
+ cv::KeyPoint kp24 = cv::KeyPoint(cv::Point2f(point24(0),point24(1)), 32.0f);
+
+ Vector2s point32 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c3);
+ Vector2s point33 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c3);
+ Vector2s point34 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c3);
+ cv::KeyPoint kp32 = cv::KeyPoint(cv::Point2f(point32(0),point32(1)), 32.0f);
+ cv::KeyPoint kp33 = cv::KeyPoint(cv::Point2f(point33(0),point33(1)), 32.0f);
+ cv::KeyPoint kp34 = cv::KeyPoint(cv::Point2f(point34(0),point34(1)), 32.0f);
+
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
+ pixel_noise_std = 1.0; //params->pixel_noise_std;
+ Vector2s pix(0,0);
+ Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2));
+
+ //create features
+ f12 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp12, 0, des, pix_cov));
+ f13 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp13, 0, des, pix_cov));
+ f14 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp14, 0, des, pix_cov));
+
+ f22 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp22, 0, des, pix_cov));
+ f23 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp23, 0, des, pix_cov));
+ f24 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp24, 0, des, pix_cov));
+
+ f32 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp32, 0, des, pix_cov));
+ f33 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp33, 0, des, pix_cov));
+ f34 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp34, 0, des, pix_cov));
+
+ //create landmarks
+ L2 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP2, camera, des));
+ L3 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP3, camera, des));
+ L4 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP4, camera, des));
+
+ //create factors
+ c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/));
+ c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/));
+ c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/));
+
+ c22 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f22, f22, L2, nullptr /*proc*/));
+ c23 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f23, f23, L3, nullptr /*proc*/));
+ c24 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f24, f24, L4, nullptr /*proc*/));
+
+ c32 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f32, f32, L2, nullptr /*proc*/));
+ c33 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f33, f33, L3, nullptr /*proc*/));
+ c34 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f34, f34, L4, nullptr /*proc*/));
+
+ ASSERT_TRUE(problem->check());
+
+// S->fix();
+ F1->fix();
+// F2->fix();
+// F3->fix();
+ L1->fix();
+// L2->fix();
+// L3->fix();
+// L4->fix();
+
+ auto p1 = F1->getP()->getState();
+ auto p2 = F2->getP()->getState();
+ auto p3 = F3->getP()->getState();
+ auto q1 = F1->getO()->getState();
+ auto q2 = F2->getO()->getState();
+ auto q3 = F3->getO()->getState();
+ auto l1 = L1->getP()->getState();
+ auto l2 = L2->getP()->getState();
+ auto l3 = L3->getP()->getState();
+ auto l4 = L4->getP()->getState();
+
+
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ std::cout << report << std::endl;
+
+ //
+ ASSERT_MATRIX_APPROX(c11->expectation(), f11->getMeasurement(), 0.1); // 0.1 pixel error allowed
+ ASSERT_MATRIX_APPROX(c12->expectation(), f12->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c13->expectation(), f13->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c14->expectation(), f14->getMeasurement(), 0.1);
+
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), p1, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), p2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), p3, 1e-6);
+ ASSERT_MATRIX_APPROX(F1->getO()->getState(), q1, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), q2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), q3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(L1->getP()->getState(), l1, 1e-6);
+ ASSERT_MATRIX_APPROX(L2->getP()->getState(), l2, 1e-6);
+ ASSERT_MATRIX_APPROX(L3->getP()->getState(), l3, 1e-6);
+ ASSERT_MATRIX_APPROX(L4->getP()->getState(), l4, 1e-6);
+
+ // perturb states
+
+ // sensor is not obvservable: comment all perturbations and unfixes
+ // S->getP()->fix();
+ // S->getP()->unfix();
+ // S->getP()->setState(S->getP()->getState() + 0.1*Vector3s::Random());
+ // S->getO()->fix();
+ // S->getO()->unfix();
+ // S->getO()->setState((Quaternions(S->getO()->getState().data()) * exp_q(0.1*Vector3s::Random())).coeffs());
+
+ // kfs
+ for (auto kf : problem->getTrajectory()->getFrameList())
+ {
+ if (kf == F1) continue;
+
+ kf->getP()->setState(kf->getP()->getState() + 0.5*Vector3s::Random());
+ kf->getO()->setState((Quaternions(kf->getO()->getState().data()) * exp_q(0.5*Vector3s::Random())).coeffs());
+ }
+
+ // lmks
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk == L1) continue;
+
+ lmk->getP()->setState(lmk->getP()->getState() + 0.5*Vector4s::Random());
+ }
+
+ // solve again
+ report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ std::cout << report << std::endl;
+
+ problem->print(1,0,1,0);
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ std::cout << "L" << lmk->id()<< ": " << std::static_pointer_cast<LandmarkHP>(lmk)->point().transpose() << std::endl;
+ }
+
+
+ // This test checks 3 DoF (3DoF are observable).
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), p1, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), p2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), p3, 1e-6);
+ ASSERT_QUATERNION_APPROX(Quaternions(F1->getO()->getState().data()), Quaternions(q1.data()), 1e-6);
+ ASSERT_QUATERNION_APPROX(Quaternions(F2->getO()->getState().data()), Quaternions(q2.data()), 1e-6);
+ ASSERT_QUATERNION_APPROX(Quaternions(F3->getO()->getState().data()), Quaternions(q3.data()), 1e-6);
+
+ ASSERT_MATRIX_APPROX(L1->point(), lmkHP1.head<3>()/lmkHP1(3), 1e-6);
+ ASSERT_MATRIX_APPROX(L2->point(), lmkHP2.head<3>()/lmkHP2(3), 1e-6);
+ ASSERT_MATRIX_APPROX(L3->point(), lmkHP3.head<3>()/lmkHP3(3), 1e-6);
+ ASSERT_MATRIX_APPROX(L4->point(), lmkHP4.head<3>()/lmkHP4(3), 1e-6);
+
+ ASSERT_MATRIX_APPROX(c11->expectation(), f11->getMeasurement(), 0.1); // 0.1 pixel error allowed
+ ASSERT_MATRIX_APPROX(c12->expectation(), f12->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c13->expectation(), f13->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c14->expectation(), f14->getMeasurement(), 0.1);
+
+
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+ ::testing::GTEST_FLAG(filter) = "FactorPixelHPTest.testSolveBundleAdjustment*";
+
return RUN_ALL_TESTS();
}
--
GitLab
From ac08feeddd47e92f4f353c8900daf505bdd6eb63 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:49:26 +0200
Subject: [PATCH 40/58] Update test processor BA
---
test/gtest_processor_bundle_adjustment.cpp | 94 ++++++++++++++++++----
1 file changed, 77 insertions(+), 17 deletions(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index c14d73d41..097a06063 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -20,7 +20,7 @@
using namespace wolf;
-std::string wolf_root = _WOLF_VISION_ROOT_DIR;
+std::string wolf_vision_root = _WOLF_VISION_ROOT_DIR;
class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{
@@ -105,7 +105,7 @@ TEST(ProcessorBundleAdjustment, installProcessor)
// Install processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -123,7 +123,7 @@ TEST(ProcessorBundleAdjustment, preProcess)
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -132,10 +132,10 @@ TEST(ProcessorBundleAdjustment, preProcess)
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr);
// demo dpreProcess
proc_dummy->preProcess();
@@ -150,7 +150,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -160,7 +160,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
proc_dummy->setLast(image_last_ptr);
@@ -169,7 +169,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
// demo detectNewFeatures
@@ -191,7 +191,7 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
// Create processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -200,9 +200,9 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
//fill feat_last list
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr);
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
@@ -234,7 +234,7 @@ TEST(ProcessorBundleAdjustment, createLandmark)
// Install processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -282,7 +282,7 @@ TEST(ProcessorBundleAdjustment, process)
// Install processor
ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
params->delete_ambiguities = true;
- params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
@@ -291,13 +291,13 @@ TEST(ProcessorBundleAdjustment, process)
auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
//1ST TIME
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
proc_dummy->process(image_inc_ptr);
ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
//2ND TIME
- CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
// proc_dummy->setInc(image_inc_ptr2);
// proc_dummy->preProcess();
// proc_dummy->getProcessKnown();
@@ -308,7 +308,7 @@ TEST(ProcessorBundleAdjustment, process)
ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
//3RD TIME -- RUNNING
- CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
assert(proc_dummy->getOrigin()!=nullptr);
assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
proc_dummy->process(image_inc_ptr3);
@@ -316,7 +316,7 @@ TEST(ProcessorBundleAdjustment, process)
ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
//4TH TIME -- RUNNING
- CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
proc_dummy->process(image_inc_ptr4);
ASSERT_EQ(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
@@ -333,6 +333,66 @@ TEST(ProcessorBundleAdjustment, processNew)
std::cout << "EMPTY Test\n";
}
+//TEST(ProcessorBundleAdjustment, processVideo)
+//{
+// // Wolf problem
+// ProblemPtr problem = Problem::create("PO", 3);
+//
+// // Install camera
+// IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+// intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
+// intr->width = 640;
+// intr->height = 480;
+// SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
+//
+// // Install processor
+// ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+// params->delete_ambiguities = true;
+// params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+// params->pixel_noise_std = 1.0;
+// params->min_track_length_for_factor = 3;
+// params->voting_active = true;
+// params->max_new_features = 400;
+// params->min_features_for_keyframe = 50;
+// auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+// auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
+//
+// //==================================vision_utils ============================================
+// vision_utils::SensorCameraPtr sen_ptr = std::make_shared<vision_utils::SensorCamera>();
+// sen_ptr->open("/home/ovendrell/eclipse-workspace/ddm_triad/data/video/VID4b.mp4"); //TODO: this should be a demo
+// //"/home/ovendrell/dev/vision_utils/src/test/data/test_usb_cam.mp4");
+//
+// unsigned int buffer_size = 10;
+// vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
+// frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
+//
+// unsigned int img_width = frame_buff.back()->getImage().cols;
+// unsigned int img_height = frame_buff.back()->getImage().rows;
+// std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
+// //==================================vision_utils ============================================
+//
+// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+// camera->setImgWidth(img_width);
+// camera->setImgHeight(img_height);
+// ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+//
+// TimeStamp t = 0;
+// // main loop
+// Scalar dt = 0.04;
+// for(int frame_count = 0; frame_count<150; ++frame_count)
+// {
+// t += dt;
+// // Image ---------------------------------------------
+// frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
+// CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
+// /* process */
+// camera->process(image);
+//// //Debug lines
+//// std::cout << "FEATURES:" << image->getFeatureList().size() << std::endl;
+//// std::cout << "MATCHES:" << image->matches_from_precedent_.size() << std::endl;
+// }
+//}
+
int main(int argc, char **argv)
--
GitLab
From e6a6e9fe80eb9f9b594cad2889b0a89325265c96 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 31 May 2019 13:59:40 +0200
Subject: [PATCH 41/58] Add opencv libs to demo BA
---
demos/CMakeLists.txt | 4 +---
1 file changed, 1 insertion(+), 3 deletions(-)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 5b7c8ec27..42a4d3048 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -12,9 +12,7 @@ else(OpenCV_FOUND)
message("[WARN] OpenCV support not installed. Minimum 2.4 version required.")
endif(OpenCV_FOUND)
-
-
ADD_EXECUTABLE(demo_processor_bundle_adjustment demo_processor_bundle_adjustment.cpp)
-TARGET_LINK_LIBRARIES(demo_processor_bundle_adjustment ${PLUGIN_NAME})
+TARGET_LINK_LIBRARIES(demo_processor_bundle_adjustment ${PLUGIN_NAME} ${OpenCV_LIBS})
--
GitLab
From 5c68270ffa10ced7858452394a89fbb9e33184fb Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Mon, 3 Jun 2019 10:44:32 +0200
Subject: [PATCH 42/58] Fix test processor BA (emplace related issue)
---
test/gtest_processor_bundle_adjustment.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 097a06063..1be6eb24a 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -261,7 +261,6 @@ TEST(ProcessorBundleAdjustment, createLandmark)
LandmarkBasePtr lmk = proc_bundle_adj->createLandmark(fea0);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
- problem_ptr->addLandmark(lmk_hp);
ASSERT_EQ(problem_ptr->getMap()->getLandmarkList().size(), 1);
}
--
GitLab
From c45703abab88dd1f552c5d19a42df0ab4526a37c Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Wed, 5 Jun 2019 10:12:12 +0200
Subject: [PATCH 43/58] Fix test processor BA
---
test/gtest_processor_bundle_adjustment.cpp | 11 ++++++-----
1 file changed, 6 insertions(+), 5 deletions(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 1be6eb24a..41a92bb75 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -303,21 +303,22 @@ TEST(ProcessorBundleAdjustment, process)
// proc_dummy->getProcessNew(params->max_new_features);
// proc_dummy->establishFactors();
proc_dummy->process(image_inc_ptr2);
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), params->max_new_features);
- ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+ ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+ ASSERT_NE(problem->getMap(), nullptr);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), true);
+
//3RD TIME -- RUNNING
CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
assert(proc_dummy->getOrigin()!=nullptr);
assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
proc_dummy->process(image_inc_ptr3);
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), params->max_new_features);
- ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+ ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
//4TH TIME -- RUNNING
CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
proc_dummy->process(image_inc_ptr4);
- ASSERT_EQ(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
+ ASSERT_LE(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
}
--
GitLab
From 45b1c903a1e724d6d71d8aa64598ac62d84cb766 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Thu, 6 Jun 2019 13:33:09 +0200
Subject: [PATCH 44/58] delete test processVideo from processor BA (should be a
demo)
---
test/gtest_processor_bundle_adjustment.cpp | 62 ----------------------
1 file changed, 62 deletions(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 41a92bb75..36bb2de9f 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -333,68 +333,6 @@ TEST(ProcessorBundleAdjustment, processNew)
std::cout << "EMPTY Test\n";
}
-//TEST(ProcessorBundleAdjustment, processVideo)
-//{
-// // Wolf problem
-// ProblemPtr problem = Problem::create("PO", 3);
-//
-// // Install camera
-// IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
-// intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
-// intr->width = 640;
-// intr->height = 480;
-// SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
-//
-// // Install processor
-// ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
-// params->delete_ambiguities = true;
-// params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
-// params->pixel_noise_std = 1.0;
-// params->min_track_length_for_factor = 3;
-// params->voting_active = true;
-// params->max_new_features = 400;
-// params->min_features_for_keyframe = 50;
-// auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
-// auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
-//
-// //==================================vision_utils ============================================
-// vision_utils::SensorCameraPtr sen_ptr = std::make_shared<vision_utils::SensorCamera>();
-// sen_ptr->open("/home/ovendrell/eclipse-workspace/ddm_triad/data/video/VID4b.mp4"); //TODO: this should be a demo
-// //"/home/ovendrell/dev/vision_utils/src/test/data/test_usb_cam.mp4");
-//
-// unsigned int buffer_size = 10;
-// vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
-// frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
-//
-// unsigned int img_width = frame_buff.back()->getImage().cols;
-// unsigned int img_height = frame_buff.back()->getImage().rows;
-// std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
-// //==================================vision_utils ============================================
-//
-// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
-// camera->setImgWidth(img_width);
-// camera->setImgHeight(img_height);
-// ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
-//
-// TimeStamp t = 0;
-// // main loop
-// Scalar dt = 0.04;
-// for(int frame_count = 0; frame_count<150; ++frame_count)
-// {
-// t += dt;
-// // Image ---------------------------------------------
-// frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
-// CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
-// /* process */
-// camera->process(image);
-//// //Debug lines
-//// std::cout << "FEATURES:" << image->getFeatureList().size() << std::endl;
-//// std::cout << "MATCHES:" << image->matches_from_precedent_.size() << std::endl;
-// }
-//}
-
-
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
--
GitLab
From 0da12f418c72da8f7db01e8916019cc53f843c60 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Thu, 6 Jun 2019 13:34:50 +0200
Subject: [PATCH 45/58] resize processor BA debug view
---
src/processor/processor_bundle_adjustment.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index cd5ce5347..bee5cf4b9 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -166,7 +166,8 @@ void ProcessorBundleAdjustment::postProcess()
cv::circle(image_debug_, point, 3, cv::Scalar(0,0,255) , 1 , 8);
}
}
-
+ cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL);
+ cv::resizeWindow("DEBUG VIEW", 800,600);
cv::imshow("DEBUG VIEW", image_debug_);
cv::waitKey(1);
}
--
GitLab
From c66408563a9ab9d030fba0d52de1518fc44f0791 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Thu, 6 Jun 2019 13:38:51 +0200
Subject: [PATCH 46/58] Add function isInlier to processor BA
---
.../vision/processor/processor_bundle_adjustment.h | 1 +
src/processor/processor_bundle_adjustment.cpp | 13 +++++++++++++
2 files changed, 14 insertions(+)
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index c13f4cad3..54909e09c 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -118,6 +118,7 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
*/
virtual bool voteForKeyFrame() override;
+ bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
bool is_tracked(int& kp_idx_);
/** \brief Detect new Features
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index bee5cf4b9..6afce4b00 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -206,6 +206,19 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
return _features_incoming_out.size();
}
+bool ProcessorBundleAdjustment::isInlier(const cv::KeyPoint& _kp_last, const cv::KeyPoint& _kp_incoming)
+{
+ // List of conditions
+ bool inlier = true;
+
+ // A. Check euclidean norm
+ inlier = inlier && (cv::norm(_kp_incoming.pt-_kp_last.pt) < mat_ptr_->getParams()->max_match_euclidean_dist);
+
+ // B. add your new condition test here
+ // inlier = inlier && ...
+
+ return inlier;
+}
bool ProcessorBundleAdjustment::is_tracked(int& _kp_idx)
{
--
GitLab
From b684f83778eff7395a54b5c48412cb66fe293eb7 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 7 Jun 2019 14:17:09 +0200
Subject: [PATCH 47/58] Add grid features to processor BA
---
.../processor/processor_bundle_adjustment.h | 33 ++-
src/processor/processor_bundle_adjustment.cpp | 197 +++++++++++++-----
test/gtest_processor_bundle_adjustment.cpp | 17 +-
3 files changed, 192 insertions(+), 55 deletions(-)
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index 54909e09c..56210ccd7 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -28,12 +28,18 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsBundleAdjustment);
struct ProcessorParamsBundleAdjustment : public ProcessorParamsTrackerFeature
{
- std::string yaml_file_params_vision_utils;
+ std::string yaml_file_params_vision_utils;
- bool delete_ambiguities;
+ bool delete_ambiguities;
- Scalar pixel_noise_std; ///< std noise of the pixel
- int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor
+ int n_cells_h;
+ int n_cells_v;
+ int min_response_new_feature;
+
+ Scalar pixel_noise_std; ///< std noise of the pixel
+ int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor
+
+ ProcessorParamsBundleAdjustment() = default;
};
@@ -165,6 +171,15 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
* \brief Return Image for debug purposes
*/
cv::Mat getImageDebug();
+
+ /**
+ * \brief Return list of Features tracked in a Capture
+ */
+ std::list<FeatureBasePtr> trackedFeatures(const CaptureBasePtr& _capture_ptr);
+ /**
+ * \brief Return list of Landmarks
+ */
+ std::list<LandmarkBasePtr> currentLandmarks();
};
inline cv::Mat ProcessorBundleAdjustment::getImageDebug()
@@ -172,6 +187,16 @@ inline cv::Mat ProcessorBundleAdjustment::getImageDebug()
return image_debug_;
}
+inline std::list<FeatureBasePtr> ProcessorBundleAdjustment::trackedFeatures(const CaptureBasePtr& _capture_ptr)
+{
+ return track_matrix_.snapshotAsList(_capture_ptr);
+}
+
+inline std::list<LandmarkBasePtr> ProcessorBundleAdjustment::currentLandmarks()
+{
+ return getProblem()->getMap()->getLandmarkList();
+}
+
} //namespace wolf
#endif /* INCLUDE_BASE_PROCESSOR_PROCESSOR_BUNDLE_ADJUSTMENT_H_ */
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 6afce4b00..5c0589c26 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -55,9 +55,23 @@ void ProcessorBundleAdjustment::preProcess()
capture_image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
// Detect KeyPoints
capture_image_incoming_->keypoints_ = det_ptr_->detect(capture_image_incoming_->getImage());
+
+ //Sort keypoints by response if needed
+ if (!capture_image_incoming_->keypoints_.empty())
+ vision_utils::sortByResponse(capture_image_incoming_->keypoints_, CV_SORT_DESCENDING);
+
// Compute Descriptors
capture_image_incoming_->descriptors_ = des_ptr_->getDescriptor(capture_image_incoming_->getImage(), capture_image_incoming_->keypoints_);
+ std::cout << "before fill grid " << std::endl;
+
+ // Create and fill incoming grid
+ capture_image_incoming_->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(capture_image_incoming_->getImage().rows, capture_image_incoming_->getImage().cols, params_bundle_adjustment_->n_cells_v, params_bundle_adjustment_->n_cells_h);
+
+ capture_image_incoming_->grid_features_->insert(capture_image_incoming_->keypoints_);
+
+ std::cout << "after fill grid done " << std::endl;
+
if (last_ptr_ != nullptr)
{
@@ -90,7 +104,20 @@ void ProcessorBundleAdjustment::preProcess()
capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
}
+
+// cv::Mat img_mat = cv::Mat(capture_image_incoming_->getImage().rows, capture_image_incoming_->getImage().cols, capture_image_incoming_->getImage().type());
+//
+// cv::drawMatches(capture_image_incoming_->getImage(), capture_image_incoming_->getKeypoints(),
+// capture_image_last_->getImage(), capture_image_last_->getKeypoints(),
+// capture_image_incoming_->matches_from_precedent_, img_mat);
+//
+// cv::namedWindow("MATCHES VIEW", cv::WINDOW_NORMAL);
+// cv::resizeWindow("MATCHES VIEW", 800,600);
+// cv::imshow("MATCHES VIEW", img_mat);
}
+
+ std::cout << "preProcess() done " << std::endl;
+
}
void ProcessorBundleAdjustment::postProcess()
@@ -122,26 +149,28 @@ void ProcessorBundleAdjustment::postProcess()
// DEBUG
image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks);
-// typedef std::map<size_t, LandmarkBasePtr>::iterator iter;
-// for(iter it = lmk_track_map_.begin(); it != lmk_track_map_.end(); ++it)
-// {
-// Vector3s point3D = std::static_pointer_cast<LandmarkHP>(it->second)->point();
-// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
-// Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D);
-// cv::Point point = cv::Point(point2D(0), point2D(1));
-// cv::circle(image_debug_, point, 1, cv::Scalar(0,0,255) , 1 , 8);
-// }
-
-// for (auto const & feat_base : last_ptr_->getFeatureList())
-// {
-// FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
-// unsigned int track_id = feat->trackId();
-// Vector3s point3D = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
-// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
-// Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D);
-// cv::Point point = cv::Point(point2D(0), point2D(1));
-// cv::circle(image_debug_, point, 1, cv::Scalar(0,0,255) , 1 , 8);
-// }
+ Snapshot snapshot_last = track_matrix_.snapshot(last_ptr_);
+// getProblem()->print(4,1,1,0);
+ for (auto pair : track_matrix_.snapshot(origin_ptr_))
+ {
+ if (snapshot_last.count(pair.first)==0)
+ {
+ if (!(pair.second->getFactorList()).empty())
+ {
+ auto factor = pair.second->getFactorList().front();
+ if (factor)
+ {
+ auto lmk = factor->getLandmarkOther();
+ if (lmk)
+ {
+// lmk->remove();
+// track_matrix_.remove(pair.second->trackId());
+ }
+ }
+ }
+ }
+ }
+ getProblem()->print(4,1,1,0);
list<FeatureBasePtr> snapshot = track_matrix_.snapshotAsList(last_ptr_);
for (auto const & feat_base : snapshot)
@@ -187,18 +216,25 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
{
// Get kp incoming and keypoint last
cv::KeyPoint kp_inc = capture_image_incoming_->keypoints_.at(index_inc);
- //cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(feat_last_->getIndexKeyPoint());
+// cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(feat_last->getIndexKeyPoint());
- FeaturePointImagePtr feat_inc = std::make_shared<FeaturePointImage>(kp_inc, index_inc,
- capture_image_incoming_->descriptors_.row(index_inc),
- pixel_cov_);
+// if (isInlier(kp_last, kp_inc))
+// {
+ FeaturePointImagePtr feat_inc = std::make_shared<FeaturePointImage>(kp_inc, index_inc,
+ capture_image_incoming_->descriptors_.row(index_inc),
+ pixel_cov_);
- _features_incoming_out.push_back(feat_inc);
+ _features_incoming_out.push_back(feat_inc);
- auto feature_match_ptr = std::make_shared<FeatureMatch>();
- feature_match_ptr->feature_ptr_= feat_last;
- feature_match_ptr->normalized_score_ = 1.0;//capture_image_incoming_->matches_normalized_scores_.at(index_inc);
- _feature_correspondences[feat_inc] = feature_match_ptr;
+ auto feature_match_ptr = std::make_shared<FeatureMatch>();
+ feature_match_ptr->feature_ptr_= feat_last;
+ feature_match_ptr->normalized_score_ = 1.0;//capture_image_incoming_->matches_normalized_scores_.at(index_inc);
+ _feature_correspondences[feat_inc] = feature_match_ptr;
+
+
+ // hit cell to acknowledge there's a tracked point in that cell
+ capture_image_incoming_->grid_features_->hitTrackingCell(kp_inc);
+// }
}
}
}
@@ -238,27 +274,81 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
{
//TODO: efficient implementation?
- typedef std::map<int,int>::iterator iter;
-
- for (iter it = capture_image_last_->map_index_to_next_.begin(); it!=capture_image_last_->map_index_to_next_.end(); ++it)
- {
- if (_features_last_out.size() >= _max_new_features)
- break;
-
- else if(!is_tracked(it->second))
- {
- //add to _features_last_out
- int idx_last = it->first;
- cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(idx_last);
- FeaturePointImagePtr feat_last_ = std::make_shared<FeaturePointImage>(kp_last, idx_last,
- capture_image_last_->descriptors_.row(idx_last),
- pixel_cov_);
- _features_last_out.push_back(feat_last_);
-
- }
- }
-
- return _features_last_out.size();
+////Simpler method to detect new features without using Grid Features form vision_utils
+// typedef std::map<int,int>::iterator iter;
+//
+// for (iter it = capture_image_last_->map_index_to_next_.begin(); it!=capture_image_last_->map_index_to_next_.end(); ++it)
+// {
+// if (_features_last_out.size() >= _max_new_features)
+// break;
+//
+// else if(!is_tracked(it->second))
+// {
+// //add to _features_last_out
+// int idx_last = it->first;
+// cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(idx_last);
+// FeaturePointImagePtr feat_last_ = std::make_shared<FeaturePointImage>(kp_last, idx_last,
+// capture_image_last_->descriptors_.row(idx_last),
+// pixel_cov_);
+// _features_last_out.push_back(feat_last_);
+//
+// }
+// }
+//
+// return _features_last_out.size();
+
+ for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; ++n_iterations)
+ {
+ Eigen::Vector2i cell_last;
+ assert(capture_image_last_->grid_features_!=nullptr);
+ if (capture_image_last_->grid_features_->pickEmptyTrackingCell(cell_last))
+ {
+ // Get best keypoint in cell
+ vision_utils::FeatureIdxMap cell_feat_map = capture_image_last_->grid_features_->getFeatureIdxMap(cell_last(0), cell_last(1), params_bundle_adjustment_->min_response_new_feature);
+ bool found_feature_in_cell = false;
+
+ for (auto target_last_pair_response_idx : cell_feat_map)
+ {
+ // Get KeyPoint in last
+ unsigned int index_last = target_last_pair_response_idx.second;
+ cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(index_last);
+ assert(target_last_pair_response_idx.first == kp_last.response && "[ProcessorTrackerFeatureTrifocal::detectNewFeatures]: response mismatch.");
+
+ // Check if there is match with incoming, if not we do not want it
+ if (capture_image_last_->map_index_to_next_.count(index_last))
+ {
+ // matching keypoint in incoming
+// unsigned int index_incoming = capture_image_last_->map_index_to_next_[index_last];
+// cv::KeyPoint kp_incoming = capture_image_incoming_->keypoints_.at(index_incoming);
+
+ // validate match with extra tests
+// if (isInlier( kp_incoming, kp_last))
+// {
+ // Create WOLF feature
+ FeaturePointImagePtr ftr_point_last = std::make_shared<FeaturePointImage>(
+ kp_last,
+ index_last,
+ capture_image_last_->descriptors_.row(index_last),
+ pixel_cov_);
+
+ _features_last_out.push_back(ftr_point_last);
+
+ // hit cell to acknowledge there's a tracked point in that cell
+ capture_image_last_->grid_features_->hitTrackingCell(kp_last);
+
+ found_feature_in_cell = true;
+
+ break; // Good kp found for this grid.
+// }
+ }
+ }
+ if (!found_feature_in_cell)
+ capture_image_last_->grid_features_->blockTrackingCell(cell_last);
+ }
+ else
+ break; // There are no empty cells
+ }
+ return _features_last_out.size();
}
@@ -294,6 +384,8 @@ bool ProcessorBundleAdjustment::voteForKeyFrame()
// WOLF_TRACE("VOTE DOWN");
// if (vote_time)
// WOLF_TRACE("VOTE TIME");
+ if (vote_down)
+ WOLF_INFO("Creating KF. Number of features: ", incoming_ptr_->getFeatureList().size());
return vote_up || vote_down || vote_time;
}
@@ -345,6 +437,9 @@ LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _featur
//vec_homogeneous_w = vec_homogeneous;
LandmarkBasePtr lmk_hp_ptr = LandmarkBase::emplace<LandmarkHP>(getProblem()->getMap(), vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
+
+// std::cout << "LANDMARKS CREATED AND ADDED to MAP: " << getProblem()->getMap()->getLandmarkList().size() << std::endl;
+
_feature_ptr->setLandmarkId(lmk_hp_ptr->id());
return lmk_hp_ptr;
}
@@ -361,6 +456,8 @@ void ProcessorBundleAdjustment::establishFactors()
for (auto const & pair_trkid_pair : matches_origin_inc)
{
size_t trkid = pair_trkid_pair.first;
+ if (track_matrix_.trackSize(trkid)<params_bundle_adjustment_->min_track_length_for_factor)
+ continue;
FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_last = pair_trkid_pair.second.second;
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 36bb2de9f..27031c3ec 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -78,6 +78,9 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
im->keypoints_ = det_ptr_->detect(im->getImage());
// Compute Descriptors
im->descriptors_ = des_ptr_->getDescriptor(im->getImage(), im->keypoints_);
+ // Create and fill incoming grid
+ im->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(im->getImage().rows, im->getImage().cols, params_bundle_adjustment_->n_cells_v, params_bundle_adjustment_->n_cells_h);
+ im->grid_features_->insert(im->keypoints_);
}
return im;
}
@@ -128,6 +131,9 @@ TEST(ProcessorBundleAdjustment, preProcess)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
+ params->n_cells_h = 50;
+ params-> n_cells_v = 50;
+ params->min_response_new_feature = 50;
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
@@ -155,6 +161,9 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
+ params->n_cells_h = 20;
+ params-> n_cells_v = 15;
+ params->min_response_new_feature = 0;
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
@@ -183,7 +192,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
// demo detectNewFeatures
unsigned int num3 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
- ASSERT_EQ(num3, params->max_new_features);
+ ASSERT_LE(num3, params->max_new_features);
}
TEST(ProcessorBundleAdjustment, trackFeatures)
@@ -196,6 +205,9 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
+ params->n_cells_h = 50;
+ params-> n_cells_v = 50;
+ params->min_response_new_feature = 50;
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
//fill feat_last list
@@ -286,6 +298,9 @@ TEST(ProcessorBundleAdjustment, process)
params->min_track_length_for_factor = 3;
params->voting_active = true;
params->max_new_features = 5;
+ params->n_cells_h = 50;
+ params-> n_cells_v = 50;
+ params->min_response_new_feature = 50;
auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
--
GitLab
From 9d3a326e36dc23da31f80621114da6643e0939f0 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 7 Jun 2019 14:17:28 +0200
Subject: [PATCH 48/58] Update demo processor BA
---
demos/demo_processor_bundle_adjustment.cpp | 100 +++------------------
1 file changed, 10 insertions(+), 90 deletions(-)
diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp
index 6ef81f47e..09b8c4ce4 100644
--- a/demos/demo_processor_bundle_adjustment.cpp
+++ b/demos/demo_processor_bundle_adjustment.cpp
@@ -84,92 +84,6 @@ int main(int argc, char** argv)
{
std::cout << std::endl << "==================== processor bundle adjustment test ======================" << std::endl;
-// std::cout << "______________________________Get image from driver_________________________" << std::endl;
-// // Sensor or sensor recording
-//
-// int num_cams = 1;
-//
-// // Get own path
-// std::string argv_str(argv[0]);
-//
-// std::cout << argv[0] << "\n";
-//
-// std::string pbase = argv_str.substr(0, argv_str.find_last_of("/"));
-// std::string paramspath = pbase + "/../demos/demo_processor_bundle_adjustment_params.xml";
-//
-// std::cout << paramspath << "\n";
-//
-// std::cout << std::endl;
-// std::cout << "******************************" << std::endl;
-// std::cout << "* ---- mvBlueFOX3 ---- *" << std::endl;
-// std::cout << "******************************" << std::endl;
-// std::cout << std::endl;
-// std::cout << "Please, choose the device to test from the following list:" << std::endl;
-// std::cout << std::endl;
-//
-// std::vector<std::string> serial;
-// serial.resize(num_cams);
-// serial[0] = GetSerialFromUser();
-//
-// std::vector<std::string> window_name;
-// window_name.resize(num_cams);
-// window_name[0] = serial[0];
-//
-//
-// mvIMPACT::acquire::DeviceManager devMgr_; // Device Manager.
-// std::vector<CMvbluefox3::CMvbluefox3*> cam_ptr; // Device.
-//
-// try {
-// cam_ptr.resize(num_cams);
-// for (int ii = 0; ii < num_cams; ++ii)
-// {
-// cam_ptr[ii] = NULL;
-//
-// if( !serial[ii].empty() )
-// {
-// std::cout << "Trying to open device with serial: " << serial[ii] << std::endl;
-//
-// if (devMgr_.getDeviceBySerial(serial[ii]) == 0)
-// std::cout << "No device found! Unable to continue. " << std::endl;
-// else
-// cam_ptr[ii] = new CMvbluefox3::CMvbluefox3(devMgr_.getDeviceBySerial(serial[ii]),paramspath);
-// }
-// else
-// std::cout << "Device cannot be found because serial number is not specified." << std::endl;
-// }
-//
-//// std::cout << "Acquiring images." << std::endl << ">WARNING: depending on OpenCV compilation flags, visualization might experience hard delays (e.g. if \"init done\" appears in the following line)" << std::endl;
-// for (int ii = 0; ii < num_cams; ++ii)
-// cv::namedWindow( window_name[ii], cv::WINDOW_NORMAL );
-//
-// while (true)
-// {
-// for (int ii = 0; ii < num_cams; ++ii)
-// {
-// cv::Mat image;
-// cam_ptr[ii]->GetImageCV(image);
-// cv::imshow( window_name[ii], image );
-// }
-// if(cv::waitKey(30) >= 0) break;
-// }
-//
-// cv::destroyAllWindows();
-//
-// std::cout << "[Camera test]: Finished successfully." << std::endl;
-// }catch (CMvbluefox3::CmvBlueFOX3Exception& e) {
-// std::cout << e.what() << std::endl;
-// }
-//
-// for (int ii = 0; ii < num_cams; ++ii)
-// {
-// if (cam_ptr[ii] != NULL)
-// {
-// cam_ptr[ii] = NULL;
-// delete [] cam_ptr[ii];
-// }
-// }
-//
-
vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
if (sen_ptr==NULL)
return 0;
@@ -191,6 +105,7 @@ int main(int argc, char** argv)
options.function_tolerance = 1e-4;
CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, options);
+
// Install camera
// IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
// intr->pinhole_model_raw = Eigen::Vector4s(320,240,320,320);
@@ -209,9 +124,12 @@ int main(int argc, char** argv)
params->pixel_noise_std = 1.0;
params->min_track_length_for_factor = 3;
params->voting_active = true;
- params->max_new_features = 200;
- params->min_features_for_keyframe = 50;
+ params->max_new_features = 15;
+ params->min_features_for_keyframe = 5;
params->time_tolerance = 0.01;
+ params->n_cells_h = 13;
+ params-> n_cells_v = 10;
+ params->min_response_new_feature = -1;
auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
@@ -242,12 +160,14 @@ int main(int argc, char** argv)
std::cout << report << std::endl;
if (number_of_KFs > 5)
break;
-
}
+// problem->print(4,1,1,0);
+
+ cv::waitKey();//1e7);
}
- problem->print(1,0,1,0);
+// problem->print(1,0,1,0);
return 0;
}
--
GitLab
From 0e23bf8a55aada1f029261c8c7c20b77944c15bc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 13 Jun 2019 14:15:08 +0200
Subject: [PATCH 49/58] opencv error in gtest_bundle_adjustment
---
.../processor/processor_bundle_adjustment.h | 39 ++++++++++----
src/processor/processor_bundle_adjustment.cpp | 51 +++++++++++--------
test/gtest_factor_pixelHP.cpp | 9 +---
test/gtest_processor_bundle_adjustment.cpp | 16 +++---
4 files changed, 66 insertions(+), 49 deletions(-)
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index 56210ccd7..b32ffbf93 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -101,12 +101,18 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
*/
virtual void postProcess() override;
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * \return the number of features tracked
*/
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) override;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) override;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
@@ -127,27 +133,38 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
bool is_tracked(int& kp_idx_);
+
/** \brief Detect new Features
- * \param _max_features maximum number of features detected
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets _features_last_out, the list of newly detected features in Capture last.
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`),
+ * or create them (`make_shared()`) and link all the returned features in _features_out (`FeatureBase::link(_capture)`).
+ *
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) override;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
- /** \brief Create a new factor and link it to the wolf tree
+ /** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
- * This function creates a factor of the appropriate type for the derived processor.
+ * This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
/** \brief Establish factors between features in Captures \b last and \b origin
*/
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 5c0589c26..e7bc2b1b0 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -202,9 +202,12 @@ void ProcessorBundleAdjustment::postProcess()
}
-unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences)
+unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
{
- for (auto feat_base: _features_last_in)
+ for (auto feat_base: _features_in)
{
FeaturePointImagePtr feat_last = std::static_pointer_cast<FeaturePointImage>(feat_base);
@@ -220,11 +223,13 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
// if (isInlier(kp_last, kp_inc))
// {
- FeaturePointImagePtr feat_inc = std::make_shared<FeaturePointImage>(kp_inc, index_inc,
- capture_image_incoming_->descriptors_.row(index_inc),
- pixel_cov_);
+ FeatureBasePtr feat_inc = FeatureBase::emplace<FeaturePointImage>(_capture,
+ kp_inc,
+ index_inc,
+ capture_image_incoming_->descriptors_.row(index_inc),
+ pixel_cov_);
- _features_incoming_out.push_back(feat_inc);
+ _features_out.push_back(feat_inc);
auto feature_match_ptr = std::make_shared<FeatureMatch>();
feature_match_ptr->feature_ptr_= feat_last;
@@ -239,7 +244,7 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
}
}
// return _feature_correspondences.size();
- return _features_incoming_out.size();
+ return _features_out.size();
}
bool ProcessorBundleAdjustment::isInlier(const cv::KeyPoint& _kp_last, const cv::KeyPoint& _kp_incoming)
@@ -270,7 +275,9 @@ bool ProcessorBundleAdjustment::is_tracked(int& _kp_idx)
return false;
}
-unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
+unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
{
//TODO: efficient implementation?
@@ -279,23 +286,23 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
//
// for (iter it = capture_image_last_->map_index_to_next_.begin(); it!=capture_image_last_->map_index_to_next_.end(); ++it)
// {
-// if (_features_last_out.size() >= _max_new_features)
+// if (_features_out.size() >= _max_new_features)
// break;
//
// else if(!is_tracked(it->second))
// {
-// //add to _features_last_out
+// //add to _features_out
// int idx_last = it->first;
// cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(idx_last);
// FeaturePointImagePtr feat_last_ = std::make_shared<FeaturePointImage>(kp_last, idx_last,
// capture_image_last_->descriptors_.row(idx_last),
// pixel_cov_);
-// _features_last_out.push_back(feat_last_);
+// _features_out.push_back(feat_last_);
//
// }
// }
//
-// return _features_last_out.size();
+// return _features_out.size();
for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; ++n_iterations)
{
@@ -325,13 +332,13 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
// if (isInlier( kp_incoming, kp_last))
// {
// Create WOLF feature
- FeaturePointImagePtr ftr_point_last = std::make_shared<FeaturePointImage>(
- kp_last,
- index_last,
- capture_image_last_->descriptors_.row(index_last),
- pixel_cov_);
+ FeatureBasePtr ftr_point_last = FeatureBase::emplace<FeaturePointImage>(_capture,
+ kp_last,
+ index_last,
+ capture_image_last_->descriptors_.row(index_last),
+ pixel_cov_);
- _features_last_out.push_back(ftr_point_last);
+ _features_out.push_back(ftr_point_last);
// hit cell to acknowledge there's a tracked point in that cell
capture_image_last_->grid_features_->hitTrackingCell(kp_last);
@@ -348,7 +355,7 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
else
break; // There are no empty cells
}
- return _features_last_out.size();
+ return _features_out.size();
}
@@ -391,7 +398,7 @@ bool ProcessorBundleAdjustment::voteForKeyFrame()
}
-FactorBasePtr ProcessorBundleAdjustment::createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+FactorBasePtr ProcessorBundleAdjustment::emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
{
/* This function is no creating any factor.
* We create factors with establishFactors()
@@ -399,7 +406,7 @@ FactorBasePtr ProcessorBundleAdjustment::createFactor(FeatureBasePtr _feature_pt
return FactorBasePtr();
}
-LandmarkBasePtr ProcessorBundleAdjustment::createLandmark(FeatureBasePtr _feature_ptr)
+LandmarkBasePtr ProcessorBundleAdjustment::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
Eigen::Vector2s point2D = _feature_ptr->getMeasurement();
@@ -467,7 +474,7 @@ void ProcessorBundleAdjustment::establishFactors()
if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
{
//createLandmark
- LandmarkBasePtr lmk = createLandmark(feature_origin);
+ LandmarkBasePtr lmk = emplaceLandmark(feature_origin);
assert(lmk!=nullptr);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
//add Landmark to map: map[track_id] = landmarkptr
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 318226812..807d5b084 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -243,16 +243,11 @@ TEST(ProcessorFactorPixelHP, testZeroResidual)
cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
cv::Mat des = cv::Mat::zeros(1,8, CV_8U);
- FeaturePointImagePtr fea0 = std::make_shared<FeaturePointImage>(kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
- fea0->setCapture(cap0);
- cap0->addFeature(fea0);
- fea0->link(cap0);
+ FeatureBasePtr fea0 = FeatureBase::emplace<FeaturePointImage>(cap0, kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
// Landmark
- LandmarkBasePtr lmk = proc_bundle_adj->createLandmark(fea0);
+ LandmarkBasePtr lmk = proc_bundle_adj->emplaceLandmark(fea0);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
- problem_ptr->addLandmark(lmk_hp);
- lmk->link(problem_ptr->getMap());
// Factor
auto fac0 = FactorBase::emplace<FactorPixelHP>(fea0, fea0, lmk_hp, proc);
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 27031c3ec..431b9bbc7 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -174,7 +174,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
proc_dummy->setLast(image_last_ptr);
// demo detectNewFeatures
- unsigned int num = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ unsigned int num = proc_dummy->detectNewFeatures(params->max_new_features, image_last_ptr, feat_list);
ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_
@@ -182,7 +182,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
proc_dummy->setInc(image_inc_ptr);
// demo detectNewFeatures
- unsigned int num2 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ unsigned int num2 = proc_dummy->detectNewFeatures(params->max_new_features, image_last_ptr, feat_list);
ASSERT_EQ(num2, 0);
// preProcess Incoming to fill last_ptr_ map_index_to_next_
@@ -191,7 +191,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_NE(last->map_index_to_next_.size(),0);
// demo detectNewFeatures
- unsigned int num3 = proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ unsigned int num3 = proc_dummy->detectNewFeatures(params->max_new_features, image_last_ptr, feat_list);
ASSERT_LE(num3, params->max_new_features);
}
@@ -218,11 +218,11 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
- proc_dummy->detectNewFeatures(params->max_new_features, feat_list);
+ proc_dummy->detectNewFeatures(params->max_new_features, last, feat_list);
//demo trackFeatures
FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
FeatureMatchMap feat_correspondance = FeatureMatchMap();
- proc_dummy->trackFeatures(feat_list, feat_list_out, feat_correspondance);
+ proc_dummy->trackFeatures(feat_list, image_inc_ptr, feat_list_out, feat_correspondance);
ASSERT_EQ(feat_list.size(), feat_list_out.size());
}
@@ -232,7 +232,7 @@ TEST(ProcessorBundleAdjustment, establishFactors)
std::cout << "EMPTY Test\n";
}
-TEST(ProcessorBundleAdjustment, createLandmark)
+TEST(ProcessorBundleAdjustment, emplaceLandmark)
{
//Build problem
ProblemPtr problem_ptr = Problem::create("PO", 3);
@@ -265,13 +265,11 @@ TEST(ProcessorBundleAdjustment, createLandmark)
cv::Mat des = cv::Mat::zeros(1,8, CV_8U);
FeaturePointImagePtr fea0 = std::make_shared<FeaturePointImage>(kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
- fea0->setCapture(cap0);
- cap0->addFeature(fea0);
fea0->link(cap0);
ASSERT_TRUE(problem_ptr->check(0));
- LandmarkBasePtr lmk = proc_bundle_adj->createLandmark(fea0);
+ LandmarkBasePtr lmk = proc_bundle_adj->emplaceLandmark(fea0);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
ASSERT_EQ(problem_ptr->getMap()->getLandmarkList().size(), 1);
--
GitLab
From 1ea5c36586fb93867d3acd8e1ea18dd6ad3b715b Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Mon, 17 Jun 2019 10:20:05 +0200
Subject: [PATCH 50/58] Detect all good kp in a cell
---
src/processor/processor_bundle_adjustment.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index e7bc2b1b0..53a86c69f 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -345,7 +345,7 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
found_feature_in_cell = true;
- break; // Good kp found for this grid.
+// break; // Good kp found for this grid. //Use to have only one keypoint per grid
// }
}
}
--
GitLab
From ba0224bd18240ed537a9aedc40e3d484847da8b5 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Mon, 17 Jun 2019 10:21:43 +0200
Subject: [PATCH 51/58] Emplace factors and landmarks depending on the track
length
---
src/processor/processor_bundle_adjustment.cpp | 25 +++++++++----------
1 file changed, 12 insertions(+), 13 deletions(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 53a86c69f..daa43b26c 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -463,30 +463,29 @@ void ProcessorBundleAdjustment::establishFactors()
for (auto const & pair_trkid_pair : matches_origin_inc)
{
size_t trkid = pair_trkid_pair.first;
+ //if track size is lower than a minimum, don't establish any factor.
if (track_matrix_.trackSize(trkid)<params_bundle_adjustment_->min_track_length_for_factor)
continue;
+
FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_last = pair_trkid_pair.second.second;
- assert(feature_origin!=feature_last && "feature: origin equal last");
- assert(feature_origin->getCapture()!=feature_last->getCapture() && "capture: origin equal last");
-
if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
{
- //createLandmark
+ //emplaceLandmark
LandmarkBasePtr lmk = emplaceLandmark(feature_origin);
- assert(lmk!=nullptr);
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
- //add Landmark to map: map[track_id] = landmarkptr
+
+ //add only one Landmark to map: map[track_id] = landmarkptr
lmk_track_map_[trkid] = lmk;
- //create factors
-// assert(feature_origin->getCapture()->getSensor() != nullptr);
-// assert(feature_origin->getCapture()->getSensorP() != nullptr);
-// assert(feature_origin->getCapture()->getSensorO() != nullptr);
-// assert(lmk->getP() != nullptr);
- FactorBase::emplace<FactorPixelHP>(feature_origin, feature_origin, lmk_hp, shared_from_this());
- FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
+ //emplace a factor for each feature in the track (only for keyframes)
+ Track full_track = track_matrix_.trackAtKeyframes(trkid);
+ for (auto it=full_track.begin(); it!=full_track.end(); ++it)
+ {
+ FactorBase::emplace<FactorPixelHP>(it->second, it->second, lmk_hp, shared_from_this());
+ }
+
}
else
{
--
GitLab
From 2a3a644b66facd9f5af6320988ba268d3e08bf07 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Mon, 17 Jun 2019 10:27:17 +0200
Subject: [PATCH 52/58] Do not delete landmarks at post process
---
src/processor/processor_bundle_adjustment.cpp | 50 +++++++++----------
1 file changed, 23 insertions(+), 27 deletions(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index daa43b26c..670177d9a 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -63,16 +63,11 @@ void ProcessorBundleAdjustment::preProcess()
// Compute Descriptors
capture_image_incoming_->descriptors_ = des_ptr_->getDescriptor(capture_image_incoming_->getImage(), capture_image_incoming_->keypoints_);
- std::cout << "before fill grid " << std::endl;
-
// Create and fill incoming grid
capture_image_incoming_->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(capture_image_incoming_->getImage().rows, capture_image_incoming_->getImage().cols, params_bundle_adjustment_->n_cells_v, params_bundle_adjustment_->n_cells_h);
capture_image_incoming_->grid_features_->insert(capture_image_incoming_->keypoints_);
- std::cout << "after fill grid done " << std::endl;
-
-
if (last_ptr_ != nullptr)
{
// Get Capture
@@ -149,28 +144,28 @@ void ProcessorBundleAdjustment::postProcess()
// DEBUG
image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks);
- Snapshot snapshot_last = track_matrix_.snapshot(last_ptr_);
-// getProblem()->print(4,1,1,0);
- for (auto pair : track_matrix_.snapshot(origin_ptr_))
- {
- if (snapshot_last.count(pair.first)==0)
- {
- if (!(pair.second->getFactorList()).empty())
- {
- auto factor = pair.second->getFactorList().front();
- if (factor)
- {
- auto lmk = factor->getLandmarkOther();
- if (lmk)
- {
-// lmk->remove();
-// track_matrix_.remove(pair.second->trackId());
- }
- }
- }
- }
- }
- getProblem()->print(4,1,1,0);
+// Snapshot snapshot_last = track_matrix_.snapshot(last_ptr_);
+//// getProblem()->print(4,1,1,0);
+// for (auto pair : track_matrix_.snapshot(origin_ptr_))
+// {
+// if (snapshot_last.count(pair.first)==0)
+// {
+// if (!(pair.second->getFactorList()).empty())
+// {
+// auto factor = pair.second->getFactorList().front();
+// if (factor)
+// {
+// auto lmk = factor->getLandmarkOther();
+// if (lmk)
+// {
+//// lmk->remove();
+//// track_matrix_.remove(pair.second->trackId());
+// }
+// }
+// }
+// }
+// }
+// //getProblem()->print(1,1,1,0);
list<FeatureBasePtr> snapshot = track_matrix_.snapshotAsList(last_ptr_);
for (auto const & feat_base : snapshot)
@@ -495,6 +490,7 @@ void ProcessorBundleAdjustment::establishFactors()
//Create factor
FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
}
+
}
}
--
GitLab
From 98bed31028e88a9ecb701329eb7c8f9fb6f48b65 Mon Sep 17 00:00:00 2001
From: oriolvendrell <oriolvendrell10@gmail.com>
Date: Fri, 21 Jun 2019 18:02:51 +0200
Subject: [PATCH 53/58] Debugging processor BA
---
.../processor/processor_bundle_adjustment.h | 14 +-
src/processor/processor_bundle_adjustment.cpp | 403 +++++++++++++++---
src/yaml/processor_bundle_adjustment_yaml.cpp | 4 +-
3 files changed, 354 insertions(+), 67 deletions(-)
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index b32ffbf93..629ee924e 100644
--- a/include/vision/processor/processor_bundle_adjustment.h
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -14,6 +14,7 @@
#include "vision/landmark/landmark_HP.h"
#include "vision/math/pinhole_tools.h"
#include "vision/sensor/sensor_camera.h"
+#include "core/math/rotations.h"
//vision utils includes
#include "vision_utils/vision_utils.h"
@@ -57,12 +58,20 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
CaptureImagePtr capture_image_last_;
CaptureImagePtr capture_image_incoming_;
+ SensorCameraPtr camera;
+
Matrix2s pixel_cov_;
+ cv::Mat tvec_;
+ cv::Mat rvec_;
+
//TODO: correct to add this?
std::map<size_t, LandmarkBasePtr> lmk_track_map_; //LandmarkTrackMap;
+ private:
+ int frame_count_;
+
public:
/** \brief Class constructor
@@ -70,7 +79,10 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_bundle_adjustment);
/** \brief Class destructor
*/
- //virtual ~ProcessorBundleAdjustment();
+ virtual ~ProcessorBundleAdjustment()
+ {
+ //
+ }
public:
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 670177d9a..82cfb37b8 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -21,9 +21,10 @@ ProcessorBundleAdjustment::ProcessorBundleAdjustment(ProcessorParamsBundleAdjust
ProcessorTrackerFeature("TRACKER BUNDLE ADJUSTMENT", _params_bundle_adjustment),
params_bundle_adjustment_(_params_bundle_adjustment),
capture_image_last_(nullptr),
- capture_image_incoming_(nullptr)
+ capture_image_incoming_(nullptr),
+ frame_count_(0)
{
-
+ //Initialize detector-descriptor-matcher
pixel_cov_ = Eigen::Matrix2s::Identity() * params_bundle_adjustment_->pixel_noise_std * params_bundle_adjustment_->pixel_noise_std;
// Detector yaml file
@@ -41,11 +42,28 @@ ProcessorBundleAdjustment::ProcessorBundleAdjustment(ProcessorParamsBundleAdjust
// Create Matcher
mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_bundle_adjustment_->yaml_file_params_vision_utils);
+ //Initialize rvec and tvec
+ tvec_ = cv::Mat::zeros(cv::Size(3,1), CV_32F);
+ rvec_ = cv::Mat::zeros(cv::Size(3,1), CV_32F);
+// Eigen::Vector3s last_pos = capture_image_last_->getFrame()->getP()->getState();
+// Eigen::Vector4s last_or = capture_image_last_->getFrame()->getO()->getState();
+// Eigen::Quaternion<Scalar> last_q(last_or(0), last_or(1), last_or(2), last_or(3));
+//
+// Eigen::VectorXs tvec_eigen = last_pos;
+// Eigen::VectorXs rvec_eigen = q2v(last_q);
+//
+// cv::eigen2cv(tvec_eigen, tvec);
+// cv::eigen2cv(rvec_eigen, rvec);
+
+
}
void ProcessorBundleAdjustment::configure(SensorBasePtr _sensor)
{
//TODO: Implement if needed
+ //Initialize camera sensor pointer
+ camera = std::static_pointer_cast<SensorCamera>(_sensor);
+
}
void ProcessorBundleAdjustment::preProcess()
@@ -76,6 +94,8 @@ void ProcessorBundleAdjustment::preProcess()
capture_image_incoming_->matches_normalized_scores_ = mat_ptr_->robustMatch(capture_image_incoming_->keypoints_, capture_image_last_->keypoints_,
capture_image_incoming_->descriptors_, capture_image_last_->descriptors_,
des_ptr_->getSize(), capture_image_incoming_->matches_from_precedent_);
+
+
//TODO: Get only the best ones
if (params_bundle_adjustment_->delete_ambiguities) //filter ambiguities
{
@@ -100,6 +120,50 @@ void ProcessorBundleAdjustment::preProcess()
}
+
+// //get points 2D from inlier matches
+// PointVector pts1, pts2;
+// for (auto match : capture_image_incoming_->matches_from_precedent_)
+// {
+// pts1.push_back(capture_image_incoming_->keypoints_[match.queryIdx].pt);
+// pts2.push_back(capture_image_last_->keypoints_[match.trainIdx].pt);
+// }
+//
+// //find Essential Matrix
+// std::vector<uchar> inliers(pts1.size(),0);
+//
+// auto camera_mat = std::static_pointer_cast<SensorCamera>(capture_image_last_->getSensor())->getIntrinsicMatrix();
+// cv::Mat camera_mat_cv;
+// cv::eigen2cv(camera_mat, camera_mat_cv);
+//
+// cv::Mat E = cv::findEssentialMat(pts1,
+// pts2,
+// camera_mat_cv,
+// CV_RANSAC,
+// 0.999,
+// 1.0,
+// inliers);
+//
+// //Compute rotation R and translation t from E
+// cv::Mat R_cv;
+// cv::Mat t_cv;
+//
+// cv::recoverPose(E,pts1,pts2, camera_mat_cv, R_cv, t_cv, inliers);
+//
+// //Convert R and t from OpenCV type to Eigen type and convert Rotation to a quaternion
+// Eigen::Matrix3s R;
+// cv::cv2eigen(R_cv, R);
+//// auto q = R2q(R); //Quaternion q
+//
+// Eigen::Vector3s t; //Translation t
+// cv::cv2eigen(t_cv,t);
+
+
+
+
+
+
+
// cv::Mat img_mat = cv::Mat(capture_image_incoming_->getImage().rows, capture_image_incoming_->getImage().cols, capture_image_incoming_->getImage().type());
//
// cv::drawMatches(capture_image_incoming_->getImage(), capture_image_incoming_->getKeypoints(),
@@ -111,25 +175,187 @@ void ProcessorBundleAdjustment::preProcess()
// cv::imshow("MATCHES VIEW", img_mat);
}
- std::cout << "preProcess() done " << std::endl;
+// std::cout << "preProcess() done " << std::endl;
}
void ProcessorBundleAdjustment::postProcess()
{
- std::map<int,std::list<vision_utils::KeyPointEnhanced> > kp_enh_tracks;
- for (auto const & feat_base : last_ptr_->getFeatureList())
+ WOLF_INFO("last ", last_ptr_, "camera ", camera);
+
+ //=====================================Compute prior for the frame===============================================
+// if (capture_image_last_)
+// {
+//
+// std::vector<cv::Point3f> landmark_points;
+// std::vector<cv::Point2f> image_points;
+//
+// //get camera information
+// auto camera_mat = camera->getIntrinsicMatrix();
+//
+// cv::Mat camera_mat_cv;
+// cv::eigen2cv(camera_mat, camera_mat_cv);
+//
+// auto dist_coeffs = camera->getDistortionVector();
+//
+// Eigen::Vector5s dist;
+// switch(dist_coeffs.size())
+// {
+// case 0:
+// {
+// dist << 0,0,0,0,0;
+// break;
+// }
+// case 1:
+// {
+// dist << dist_coeffs(0),0,0,0,0;
+// break;
+// }
+// case 2:
+// {
+// dist << dist_coeffs(0),dist_coeffs(1),0,0,0;
+// break;
+// }
+// case 3:
+// {
+// dist << dist_coeffs(0),dist_coeffs(1),0,0,dist_coeffs(2);
+// break;
+// }
+// default:
+// dist << 0,0,0,0,0;
+// }
+//
+// cv::Mat dist_coeffs_cv;
+// cv::eigen2cv(dist, dist_coeffs_cv);
+//
+//
+//
+// //fill points and landmarks list
+// int debug_counter = 0;
+//
+//// for (auto & feat : capture_image_last_->getFeatureList())
+//// {
+//// for (auto & fact : feat->getFactorList())
+//// {
+//// debug_counter++;
+//// //check the number of factors to the landmark
+//// if (fact->getLandmarkOther()->getConstrainedByList().size() >= 2)
+//// {
+//// //3d points
+//// auto point3D = std::static_pointer_cast<LandmarkHP>(fact->getLandmarkOther())->point();
+//// landmark_points.push_back(cv::Point3f(point3D(0),point3D(1),point3D(2)));
+//// //2d points
+//// auto point2D = feat->getMeasurement();
+//// image_points.push_back(cv::Point2f(point2D(0), point2D(1)));
+//// }
+//// }
+//// }
+//
+// for (auto & feat : track_matrix_.snapshotAsList(capture_image_last_))
+// {
+// debug_counter++;
+// auto trkId = feat->trackId();
+// if (lmk_track_map_.count(feat->trackId()))
+// {
+// auto lmk_base = lmk_track_map_[trkId];
+//// if (lmk_base->getConstrainedByList().size() >= 2)
+// {
+// //3d points
+// auto point3D = std::static_pointer_cast<LandmarkHP>(lmk_base)->point();
+// landmark_points.push_back(cv::Point3f(point3D(0),point3D(1),point3D(2)));
+// //2d points
+// auto point2D = feat->getMeasurement();
+// image_points.push_back(cv::Point2f(point2D(0), point2D(1)));
+// }
+// }
+// }
+//
+// WOLF_INFO("Num lmks in last:", debug_counter,"lmks constrained by >= 2 fctrs: ", landmark_points.size());
+//
+// //solvePnP
+// if (landmark_points.size() > 7)
+// {
+// WOLF_INFO("before PnP ", tvec, rvec);
+// WOLF_INFO("before PnP ", last_ptr_->getFrame()->getState().transpose());
+//
+//
+// cv::solvePnP(landmark_points, image_points, camera_mat_cv, dist_coeffs_cv, rvec, tvec, true);
+// WOLF_INFO("Solve PnP Done")
+//
+// //Compute and set Robot state
+// //q_w_s Camera quaternion
+// Eigen::Vector3s rvec_eigen;
+// cv::cv2eigen(rvec, rvec_eigen);
+// Eigen::Quaternion<Scalar> q_w_s = v2q(rvec_eigen);
+//
+// //p_w_s Camera position
+// Eigen::Vector3s p_w_s;
+// cv::cv2eigen(tvec, p_w_s);
+//
+// //Robot pose
+// // Eigen::Vector3s p_w_r = capture_image_last_->getFrame()->getP()->getState();
+// // Eigen::Quaternion<Scalar> q_w_r = Quaternions(capture_image_last_->getFrame()->getO()->getState().data());
+//
+// //Extrinsics (camera in robot reference frame)
+// Eigen::Vector3s p_r_s = camera->getP()->getState();
+// Eigen::Quaternion<Scalar> q_r_s = Quaternions(camera->getO()->getState().data());
+//
+// //Robot pose compositions
+// Eigen::Quaternion<Scalar> q_w_r = q_w_s * q_r_s.conjugate();
+// Eigen::Vector3s p_w_r = p_w_s - q_w_r * p_r_s;
+//
+//
+//
+// Eigen::Vector7s prior_state;
+// prior_state << p_w_r(0), p_w_r(1), p_w_r(2), q_w_r.x(), q_w_r.y(), q_w_r.z(), q_w_r.w();
+//
+// last_ptr_->getFrame()->setState(prior_state);
+//
+// WOLF_INFO("after PnP ", last_ptr_->getFrame()->getState().transpose());
+// WOLF_INFO("after PnP ", tvec, rvec);
+//
+// }
+//
+// //=================================================================================================================
+// }
+
+ //delete landmarks
+ getProblem()->check(1);
+ for (auto & lmk : getProblem()->getMap()->getLandmarkList())
+ {
+ if (lmk->getConstrainedByList().size()==1)
+ {
+ if (lmk->getConstrainedByList().front()->getFeature()->getCapture() != last_ptr_)
+ {
+ WOLF_INFO("Removing landmark: ", lmk->id());
+ lmk_track_map_.erase(lmk->getConstrainedByList().front()->getFeature()->trackId());
+ WOLF_INFO("Lmk deleted from track map: ", lmk->id());
+ lmk->remove();
+ WOLF_INFO("Lmk deleted: ", lmk->id());
+ }
+ }
+ }
+ getProblem()->check(1);
+
+
+ // draw debug image ======================================================================================
+
+ // image to draw
+ CaptureBasePtr image_to_draw = last_ptr_;
+
+
+ std::map<int,std::list<vision_utils::KeyPointEnhanced> > kp_enh_tracks;
+ for (auto const & feat_base : image_to_draw->getFeatureList())
{
FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
unsigned int feat_id = feat->id();
unsigned int track_id = feat->trackId();
-
// tracks
std::list<vision_utils::KeyPointEnhanced> kp_enh_track;
- for (auto feat_base : track_matrix_.trackAsVector(track_id))
+ for (auto feat_base_track : track_matrix_.trackAsVector(track_id))
{
- FeaturePointImagePtr feat_img = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ FeaturePointImagePtr feat_img = std::static_pointer_cast<FeaturePointImage>(feat_base_track);
vision_utils::KeyPointEnhanced kp_enh(feat_img->getKeypoint(),
feat_id,
track_id,
@@ -140,10 +366,8 @@ void ProcessorBundleAdjustment::postProcess()
kp_enh_tracks[feat_id] = kp_enh_track;
}
-
// DEBUG
- image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks);
-
+ image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(image_to_draw))->getImage(), kp_enh_tracks);
// Snapshot snapshot_last = track_matrix_.snapshot(last_ptr_);
//// getProblem()->print(4,1,1,0);
// for (auto pair : track_matrix_.snapshot(origin_ptr_))
@@ -165,9 +389,10 @@ void ProcessorBundleAdjustment::postProcess()
// }
// }
// }
-// //getProblem()->print(1,1,1,0);
+// getProblem()->print(0,0,1,0);
+
+ list<FeatureBasePtr> snapshot = track_matrix_.snapshotAsList(image_to_draw);
- list<FeatureBasePtr> snapshot = track_matrix_.snapshotAsList(last_ptr_);
for (auto const & feat_base : snapshot)
{
FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
@@ -175,25 +400,33 @@ void ProcessorBundleAdjustment::postProcess()
if (lmk_track_map_.count(track_id))
{
Vector3s point3D = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
- Transform<Scalar,3,Isometry> T_w_r
+
+ Transform<Scalar,3,Isometry> T_w_r
= Translation<Scalar,3>(feat_base->getFrame()->getP()->getState())
* Quaternions(feat_base->getFrame()->getO()->getState().data());
Transform<Scalar,3,Isometry> T_r_c
= Translation<Scalar,3>(feat_base->getCapture()->getSensorP()->getState())
* Quaternions(feat_base->getCapture()->getSensorO()->getState().data());
+
Eigen::Matrix<Scalar, 3, 1> point3D_c = T_r_c.inverse()
* T_w_r.inverse()
* point3D;
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
- Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D_c);
+
+// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
+
+ Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D_c);
cv::Point point = cv::Point(point2D(0), point2D(1));
+
cv::circle(image_debug_, point, 3, cv::Scalar(0,0,255) , 1 , 8);
+ std::string id = std::to_string(lmk_track_map_[track_id]->id());
+ cv::putText(image_debug_, id, point, cv::FONT_HERSHEY_DUPLEX, 1.0, CV_RGB(0,255,0),2);
}
}
- cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL);
- cv::resizeWindow("DEBUG VIEW", 800,600);
- cv::imshow("DEBUG VIEW", image_debug_);
- cv::waitKey(1);
+
+// cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL);
+// cv::resizeWindow("DEBUG VIEW", 800,600);
+// cv::imshow("DEBUG VIEW", image_debug_);
+// cv::waitKey(1);
}
@@ -214,10 +447,10 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
{
// Get kp incoming and keypoint last
cv::KeyPoint kp_inc = capture_image_incoming_->keypoints_.at(index_inc);
-// cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(feat_last->getIndexKeyPoint());
+ cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(feat_last->getIndexKeyPoint());
-// if (isInlier(kp_last, kp_inc))
-// {
+ if (isInlier(kp_last, kp_inc))
+ {
FeatureBasePtr feat_inc = FeatureBase::emplace<FeaturePointImage>(_capture,
kp_inc,
index_inc,
@@ -234,7 +467,7 @@ unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList&
// hit cell to acknowledge there's a tracked point in that cell
capture_image_incoming_->grid_features_->hitTrackingCell(kp_inc);
-// }
+ }
}
}
}
@@ -320,12 +553,12 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
if (capture_image_last_->map_index_to_next_.count(index_last))
{
// matching keypoint in incoming
-// unsigned int index_incoming = capture_image_last_->map_index_to_next_[index_last];
-// cv::KeyPoint kp_incoming = capture_image_incoming_->keypoints_.at(index_incoming);
+ unsigned int index_incoming = capture_image_last_->map_index_to_next_[index_last];
+ cv::KeyPoint kp_incoming = capture_image_incoming_->keypoints_.at(index_incoming);
// validate match with extra tests
-// if (isInlier( kp_incoming, kp_last))
-// {
+ if (isInlier( kp_incoming, kp_last))
+ {
// Create WOLF feature
FeatureBasePtr ftr_point_last = FeatureBase::emplace<FeaturePointImage>(_capture,
kp_last,
@@ -340,8 +573,8 @@ unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_fe
found_feature_in_cell = true;
-// break; // Good kp found for this grid. //Use to have only one keypoint per grid
-// }
+ break; // Good kp found for this grid. //Use to have only one keypoint per grid
+ }
}
}
if (!found_feature_in_cell)
@@ -362,6 +595,17 @@ bool ProcessorBundleAdjustment::correctFeatureDrift(const FeatureBasePtr _origin
bool ProcessorBundleAdjustment::voteForKeyFrame()
{
+
+ frame_count_ ++;
+
+
+ return ((frame_count_ % 5) == 0);
+
+
+
+
+
+
// // A. crossing voting threshold with ascending number of features
bool vote_up = false;
// // 1. vote if we did not have enough features before
@@ -449,49 +693,80 @@ LandmarkBasePtr ProcessorBundleAdjustment::emplaceLandmark(FeatureBasePtr _featu
void ProcessorBundleAdjustment::establishFactors()
{
- if (origin_ptr_ == last_ptr_)
- return;
-
+// if (origin_ptr_ == last_ptr_)
+// return;
+//
+// TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, last_ptr_);
+//
+// for (auto const & pair_trkid_pair : matches_origin_inc)
+// {
+// size_t trkid = pair_trkid_pair.first;
+// //if track size is lower than a minimum, don't establish any factor.
+// if (track_matrix_.trackSize(trkid) < params_bundle_adjustment_->min_track_length_for_factor)
+// continue;
+//
+// FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
+// FeatureBasePtr feature_last = pair_trkid_pair.second.second;
+//
+// if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
+// {
+// //emplaceLandmark
+// LandmarkBasePtr lmk = emplaceLandmark(feature_origin);
+// LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+//
+// //add only one Landmark to map: map[track_id] = landmarkptr
+// lmk_track_map_[trkid] = lmk;
+//
+// //emplace a factor for each feature in the track (only for keyframes)
+// Track full_track = track_matrix_.trackAtKeyframes(trkid);
+// for (auto it=full_track.begin(); it!=full_track.end(); ++it)
+// {
+// FactorBase::emplace<FactorPixelHP>(it->second, it->second, lmk_hp, shared_from_this());
+// }
+//
+// }
+// else
+// {
+// LandmarkBasePtr lmk = lmk_track_map_[trkid];
+// LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+//
+// //Create factor
+// FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
+// }
+//
+// }
- TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, last_ptr_);
- for (auto const & pair_trkid_pair : matches_origin_inc)
- {
- size_t trkid = pair_trkid_pair.first;
- //if track size is lower than a minimum, don't establish any factor.
- if (track_matrix_.trackSize(trkid)<params_bundle_adjustment_->min_track_length_for_factor)
- continue;
- FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
- FeatureBasePtr feature_last = pair_trkid_pair.second.second;
+ for (auto & pair_id_ftr : track_matrix_.snapshot(last_ptr_))
+ {
+ auto & trkid = pair_id_ftr.first;
+ auto & ftr = pair_id_ftr.second;
- if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
- {
- //emplaceLandmark
- LandmarkBasePtr lmk = emplaceLandmark(feature_origin);
- LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+ if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
+ {
+ //emplaceLandmark
+ LandmarkBasePtr lmk = emplaceLandmark(ftr);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
- //add only one Landmark to map: map[track_id] = landmarkptr
- lmk_track_map_[trkid] = lmk;
+ //add only one Landmark to map: map[track_id] = landmarkptr
+ lmk_track_map_[trkid] = lmk;
- //emplace a factor for each feature in the track (only for keyframes)
- Track full_track = track_matrix_.trackAtKeyframes(trkid);
- for (auto it=full_track.begin(); it!=full_track.end(); ++it)
- {
- FactorBase::emplace<FactorPixelHP>(it->second, it->second, lmk_hp, shared_from_this());
- }
+ //emplace a factor
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this());
- }
- else
- {
- LandmarkBasePtr lmk = lmk_track_map_[trkid];
- LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+ }
+ else
+ {
+ // recover the landmark
+ LandmarkBasePtr lmk = lmk_track_map_[trkid];
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
- //Create factor
- FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
- }
+ //emplace a factor
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this());
+ }
- }
+ }
}
void ProcessorBundleAdjustment::setParams(const ProcessorParamsBundleAdjustmentPtr _params)
diff --git a/src/yaml/processor_bundle_adjustment_yaml.cpp b/src/yaml/processor_bundle_adjustment_yaml.cpp
index ac6740fc5..11ec27e6c 100644
--- a/src/yaml/processor_bundle_adjustment_yaml.cpp
+++ b/src/yaml/processor_bundle_adjustment_yaml.cpp
@@ -42,11 +42,11 @@ static ProcessorParamsBasePtr createProcessorParamsBundleAdjustment(const std::s
params->delete_ambiguities = algorithm["delete ambiguities"] .as<bool>();
params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
- /*
+
params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
params->n_cells_v = algorithm["grid vert cells"] .as<int>();
params->min_response_new_feature = algorithm["min response new features"] .as<int>();
- */
+
params->min_track_length_for_factor = algorithm["min track length for factor"].as<int>();
--
GitLab
From f44db5c78557a7b9b6c5d4282554d242e608c0f3 Mon Sep 17 00:00:00 2001
From: Oriol Vendrell Gallart <ovendrell@iri.upc.edu>
Date: Fri, 28 Jun 2019 09:11:01 +0200
Subject: [PATCH 54/58] Add lmk id to 2D visualization
---
src/processor/processor_bundle_adjustment.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 82cfb37b8..c32f4d866 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -419,7 +419,7 @@ void ProcessorBundleAdjustment::postProcess()
cv::circle(image_debug_, point, 3, cv::Scalar(0,0,255) , 1 , 8);
std::string id = std::to_string(lmk_track_map_[track_id]->id());
- cv::putText(image_debug_, id, point, cv::FONT_HERSHEY_DUPLEX, 1.0, CV_RGB(0,255,0),2);
+ cv::putText(image_debug_, id, point, cv::FONT_HERSHEY_DUPLEX, 0.5, CV_RGB(0,255,0),2);
}
}
--
GitLab
From 9404888571bc7d5e57c0c053e5ceb9d0280d37d4 Mon Sep 17 00:00:00 2001
From: Oriol Vendrell Gallart <ovendrell@iri.upc.edu>
Date: Fri, 28 Jun 2019 09:11:47 +0200
Subject: [PATCH 55/58] Remove lmks from lost tracks with only one factor
---
src/processor/processor_bundle_adjustment.cpp | 15 ++++++++++-----
1 file changed, 10 insertions(+), 5 deletions(-)
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index c32f4d866..38a0f1a41 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -321,22 +321,27 @@ void ProcessorBundleAdjustment::postProcess()
// }
//delete landmarks
- getProblem()->check(1);
+ getProblem()->print(4,1,0,0);
+ std::list<LandmarkBasePtr> lmks_to_remove;
for (auto & lmk : getProblem()->getMap()->getLandmarkList())
{
if (lmk->getConstrainedByList().size()==1)
{
- if (lmk->getConstrainedByList().front()->getFeature()->getCapture() != last_ptr_)
+ if (lmk->getConstrainedByList().front()->getFeature()->getCapture() != origin_ptr_)
{
WOLF_INFO("Removing landmark: ", lmk->id());
lmk_track_map_.erase(lmk->getConstrainedByList().front()->getFeature()->trackId());
WOLF_INFO("Lmk deleted from track map: ", lmk->id());
- lmk->remove();
- WOLF_INFO("Lmk deleted: ", lmk->id());
+ lmks_to_remove.push_back(lmk);
}
}
}
- getProblem()->check(1);
+ for (auto & lmk : lmks_to_remove)
+ {
+ lmk->remove();
+ WOLF_INFO("Lmk deleted: ", lmk->id());
+ }
+ getProblem()->check(0);
// draw debug image ======================================================================================
--
GitLab
From c456222f6c553abbf51f707d35dcdc027ffddc19 Mon Sep 17 00:00:00 2001
From: Oriol Vendrell Gallart <ovendrell@iri.upc.edu>
Date: Fri, 28 Jun 2019 09:12:05 +0200
Subject: [PATCH 56/58] Change demo BA parametrers
---
demos/demo_processor_bundle_adjustment.cpp | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp
index 09b8c4ce4..d7a27c2d9 100644
--- a/demos/demo_processor_bundle_adjustment.cpp
+++ b/demos/demo_processor_bundle_adjustment.cpp
@@ -122,14 +122,14 @@ int main(int argc, char** argv)
params->delete_ambiguities = true;
params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
params->pixel_noise_std = 1.0;
- params->min_track_length_for_factor = 3;
+ params->min_track_length_for_factor = 1;
params->voting_active = true;
- params->max_new_features = 15;
- params->min_features_for_keyframe = 5;
+ params->max_new_features = 200;
+ params->min_features_for_keyframe = 50;
params->time_tolerance = 0.01;
- params->n_cells_h = 13;
- params-> n_cells_v = 10;
- params->min_response_new_feature = -1;
+ params->n_cells_h = 20;
+ params-> n_cells_v = 17;
+ params->min_response_new_feature = 10;
auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
--
GitLab
From db8e857af9430b0d925ea1299844dd3517a1f808 Mon Sep 17 00:00:00 2001
From: Oriol Vendrell Gallart <ovendrell@iri.upc.edu>
Date: Fri, 28 Jun 2019 09:28:27 +0200
Subject: [PATCH 57/58] Adapt changes in process()
---
test/gtest_processor_bundle_adjustment.cpp | 12 ++++++++----
test/gtest_processor_tracker_feature_trifocal.cpp | 9 ++++++---
2 files changed, 14 insertions(+), 7 deletions(-)
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index 431b9bbc7..8fe8bfb72 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -304,7 +304,8 @@ TEST(ProcessorBundleAdjustment, process)
//1ST TIME
CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
- proc_dummy->process(image_inc_ptr);
+ image_inc_ptr->process();
+// proc_dummy->process(image_inc_ptr);
ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
@@ -315,7 +316,8 @@ TEST(ProcessorBundleAdjustment, process)
// proc_dummy->getProcessKnown();
// proc_dummy->getProcessNew(params->max_new_features);
// proc_dummy->establishFactors();
- proc_dummy->process(image_inc_ptr2);
+ image_inc_ptr2->process();
+// proc_dummy->process(image_inc_ptr2);
ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
ASSERT_NE(problem->getMap(), nullptr);
ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), true);
@@ -325,12 +327,14 @@ TEST(ProcessorBundleAdjustment, process)
CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
assert(proc_dummy->getOrigin()!=nullptr);
assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
- proc_dummy->process(image_inc_ptr3);
+ image_inc_ptr3->process();
+// proc_dummy->process(image_inc_ptr3);
ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
//4TH TIME -- RUNNING
CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
- proc_dummy->process(image_inc_ptr4);
+ image_inc_ptr4->process();
+// proc_dummy->process(image_inc_ptr4);
ASSERT_LE(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
}
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 3cc902a0c..92351ff34 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -131,7 +131,8 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
image *= 0; // TODO see if we want to use a real image
SensorCameraPtr sens_trk_cam = std::static_pointer_cast<SensorCamera>(sens_trk);
CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
- proc_trk->process(capt_trk);
+ capt_trk->process();
+// proc_trk->process(capt_trk);
problem->print(2,0,1,0);
@@ -142,11 +143,13 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
WOLF_INFO("----------------------- ts: ", t , " --------------------------");
capt_odo->setTimeStamp(t);
- proc_odo->process(capt_odo);
+ capt_odo->process();
+// proc_odo->process(capt_odo);
// Track
capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
- proc_trk->process(capt_trk);
+ capt_trk->process();
+// proc_trk->process(capt_trk);
problem->print(2,0,1,0);
--
GitLab
From 6352924ad21ceb4aa8dca689b05458e4b91eddfc Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Tue, 2 Jul 2019 15:56:42 +0200
Subject: [PATCH 58/58] remove-serialization
---
serialization/CMakeLists.txt | 1 -
serialization/cereal/CMakeLists.txt | 26 -
serialization/cereal/archives.h | 9 -
serialization/cereal/io.h | 229 -------
.../cereal/serialization_eigen_core.h | 125 ----
.../cereal/serialization_eigen_geometry.h | 38 --
.../cereal/serialization_eigen_sparse.h | 79 ---
...serialization_local_parametrization_base.h | 26 -
...zation_local_parametrization_homogeneous.h | 27 -
...ization_local_parametrization_quaternion.h | 30 -
.../cereal/serialization_node_base.h | 77 ---
.../serialization_processor_odom2d_params.h | 29 -
.../serialization_processor_odom3d_params.h | 27 -
.../serialization_processor_params_base.h | 22 -
.../serialization_sensor_intrinsic_base.h | 25 -
.../serialization_sensor_odom2d_intrinsic.h | 26 -
.../cereal/serialization_time_stamp.h | 31 -
test/serialization/CMakeLists.txt | 4 -
test/serialization/cereal/CMakeLists.txt | 43 --
.../cereal/gtest_serialization_eigen_core.cpp | 92 ---
.../gtest_serialization_eigen_geometry.cpp | 72 ---
.../gtest_serialization_eigen_sparse.cpp | 114 ----
...st_serialization_local_parametrization.cpp | 571 ------------------
.../cereal/gtest_serialization_node_base.cpp | 247 --------
..._serialization_processor_odom2d_params.cpp | 108 ----
..._serialization_processor_odom3d_params.cpp | 246 --------
.../cereal/gtest_serialization_save_load.cpp | 202 -------
...st_serialization_sensor_intrinsic_base.cpp | 233 -------
..._serialization_sensor_odom2d_intrinsic.cpp | 256 --------
.../cereal/gtest_serialization_time_stamp.cpp | 62 --
30 files changed, 3077 deletions(-)
delete mode 100644 serialization/CMakeLists.txt
delete mode 100644 serialization/cereal/CMakeLists.txt
delete mode 100644 serialization/cereal/archives.h
delete mode 100644 serialization/cereal/io.h
delete mode 100644 serialization/cereal/serialization_eigen_core.h
delete mode 100644 serialization/cereal/serialization_eigen_geometry.h
delete mode 100644 serialization/cereal/serialization_eigen_sparse.h
delete mode 100644 serialization/cereal/serialization_local_parametrization_base.h
delete mode 100644 serialization/cereal/serialization_local_parametrization_homogeneous.h
delete mode 100644 serialization/cereal/serialization_local_parametrization_quaternion.h
delete mode 100644 serialization/cereal/serialization_node_base.h
delete mode 100644 serialization/cereal/serialization_processor_odom2d_params.h
delete mode 100644 serialization/cereal/serialization_processor_odom3d_params.h
delete mode 100644 serialization/cereal/serialization_processor_params_base.h
delete mode 100644 serialization/cereal/serialization_sensor_intrinsic_base.h
delete mode 100644 serialization/cereal/serialization_sensor_odom2d_intrinsic.h
delete mode 100644 serialization/cereal/serialization_time_stamp.h
delete mode 100644 test/serialization/CMakeLists.txt
delete mode 100644 test/serialization/cereal/CMakeLists.txt
delete mode 100644 test/serialization/cereal/gtest_serialization_eigen_core.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_local_parametrization.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_node_base.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_save_load.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
delete mode 100644 test/serialization/cereal/gtest_serialization_time_stamp.cpp
diff --git a/serialization/CMakeLists.txt b/serialization/CMakeLists.txt
deleted file mode 100644
index a853ba8ec..000000000
--- a/serialization/CMakeLists.txt
+++ /dev/null
@@ -1 +0,0 @@
-add_subdirectory(cereal)
diff --git a/serialization/cereal/CMakeLists.txt b/serialization/cereal/CMakeLists.txt
deleted file mode 100644
index 1ae16bcd2..000000000
--- a/serialization/cereal/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-SET(HDRS_SERIALIZATION ${HDRS_SERIALIZATION}
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_core.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_geometry.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_sparse.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_homogeneous.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_quaternion.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_node_base.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_sensor_intrinsic_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_sensor_odom2d_intrinsic.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_params_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_odom2d_params.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_odom3d_params.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_time_stamp.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/archives.h
- ${CMAKE_CURRENT_SOURCE_DIR}/io.h
-
- PARENT_SCOPE)
diff --git a/serialization/cereal/archives.h b/serialization/cereal/archives.h
deleted file mode 100644
index d8c16fd18..000000000
--- a/serialization/cereal/archives.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_ARCHIVE_H_
-#define _WOLF_IO_CEREAL_ARCHIVE_H_
-
-#include <cereal/archives/binary.hpp>
-#include <cereal/archives/json.hpp>
-#include <cereal/archives/portable_binary.hpp>
-#include <cereal/archives/xml.hpp>
-
-#endif /* _WOLF_IO_CEREAL_ARCHIVE_H_ */
diff --git a/serialization/cereal/io.h b/serialization/cereal/io.h
deleted file mode 100644
index 8a8e58c2d..000000000
--- a/serialization/cereal/io.h
+++ /dev/null
@@ -1,229 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_IO_H_
-#define _WOLF_SERIALIZATION_CEREAL_IO_H_
-
-#include <stdexcept>
-#include "archives.h"
-
-//#include <cereal/types/tuple.hpp>
-
-namespace wolf {
-namespace serialization {
-
-/// @todo demangle typeid.name ?
-template <typename T>
-inline const std::string& type_name(const T&)
-{
- static const std::string typeid_name = typeid(T).name();
- return typeid_name;
-}
-
-inline std::string extension(const std::string& file)
-{
- const std::size_t p = file.find_last_of(".");
- return (p != std::string::npos) ? file.substr(p) : "";
-}
-
-//struct Extensions
-//{
-// constexpr static const char* bin = ".bin";
-// constexpr static const char* json = ".json";
-// constexpr static const char* xml = ".xml";
-
-// constexpr static const char* fall_back = json;
-//};
-
-//enum class Extensions2 : std::size_t
-//{
-// BIN = 0,
-//// CBIN,
-// JSON,
-//// TEXT,
-// XML,
-//};
-
-//template <char... Chars>
-//struct constexp_str
-//{
-// using type = constexp_str<Chars...>;
-
-// virtual ~constexp_str() = default;
-
-// constexpr static const char value[sizeof...(Chars)+1] = {Chars..., '\0'};
-
-// constexpr static std::size_t size() { return sizeof...(Chars); }
-
-// constexpr static const char* c_str() { return &value[0]; }
-
-//// constexpr static bool comp(const std::string& s) { return s == value; }
-
-// /*constexpr*/ bool operator == (const std::string& s) { return s == value; }
-
-// constexpr /*static*/ operator const char* () { return c_str(); }
-// constexpr /*static*/ operator std::string& () { return c_str(); }
-//};
-
-struct Extensions
-{
-// template <char... Chars>
-// struct EXT : constexp_str<Chars...>
-// {
-// //
-// };
-
-// struct BIN : EXT<'.','b','i','n'> { };
-// struct XML : EXT<'.','x','m','l'> { };
-// struct JSON : EXT<'.','j','s','o','n'> { };
-
- struct EXT { virtual ~EXT() = default; };
-
- struct BIN : EXT
- {
- constexpr static const char* value = ".bin";
- bool operator == (const std::string& s) { return value == s; }
- };
-
- struct XML : EXT
- {
- constexpr static const char* value = ".xml";
- bool operator == (const std::string& s) { return value == s; }
- };
-
- struct JSON : EXT
- {
- constexpr static const char* value = ".json";
- bool operator == (const std::string& s) { return value == s; }
- };
-};
-
-template <typename Ar>
-void serialize_pack(Ar&&)
-{
- // end of expansion
-}
-
-template <typename Ar, typename T, typename... Args>
-void serialize_pack(Ar&& archive, T&& object, Args&&... args)
-{
- archive( cereal::make_nvp(type_name(object), std::forward<T>(object)) );
- serialize_pack(archive, std::forward<Args>(args)...);
-}
-
-template <typename Ar, typename S, typename T, typename... Args>
-void serialize(S& stream, T&& object, Args&&... args)
-{
- Ar archive(stream);
- archive( cereal::make_nvp(type_name(object), std::forward<T>(object)) );
-
- serialize_pack(archive, std::forward<Args>(args)...);
-}
-
-template <typename EXT, typename InAr, typename OutAr>
-struct Serializer
-{
- template <typename S, typename... T>
- static void serialize_in(S& stream, T&... object)
- {
- serialize<InAr>(stream, object...);
- }
-
- template <typename S, typename... T>
- static void serialize_out(S& stream, T&&... object)
- {
- serialize<OutAr>(stream, std::forward<T>(object)...);
- }
-
- template <typename... T>
- static void save(std::string filename, T&&... o)
- {
- const std::string ext = serialization::extension(filename);
-
- if (ext != EXT::value) filename += EXT::value;
-
- std::ofstream os(filename);
- serialize_out(os, std::forward<T>(o)...);
- }
-
- template <typename... T>
- static void load(std::string filename, T&... o)
- {
- const std::string ext = serialization::extension(filename);
-
- if (ext != EXT::value) filename += EXT::value;
-
- std::ifstream is(filename);
- serialize_in(is, o...);
- }
-};
-
-using SerializerBin = Serializer<Extensions::BIN,
- cereal::BinaryInputArchive,
- cereal::BinaryOutputArchive>;
-
-using SerializerXML = Serializer<Extensions::XML,
- cereal::XMLInputArchive,
- cereal::XMLOutputArchive>;
-
-using SerializerJSON = Serializer<Extensions::JSON,
- cereal::JSONInputArchive,
- cereal::JSONOutputArchive>;
-
-} /* namespace serialization */
-
-template <typename... T>
-void save(const std::string& filename, T&&... o)
-throw(std::runtime_error)
-{
- const std::string ext = serialization::extension(filename);
-
- if (ext == serialization::Extensions::BIN::value)
- {
- serialization::SerializerBin::save(filename, std::forward<T>(o)...);
- }
- else if (ext == serialization::Extensions::JSON::value)
- {
- serialization::SerializerJSON::save(filename, std::forward<T>(o)...);
- }
- else if (ext == serialization::Extensions::XML::value)
- {
- serialization::SerializerXML::save(filename, std::forward<T>(o)...);
- }
- else if (ext == "") // falback is json
- {
- serialization::SerializerJSON::save(filename, std::forward<T>(o)...);
- }
- else
- {
- throw std::runtime_error("Unknown file extension : " + filename);
- }
-}
-
-template <typename... T>
-void load(const std::string& filename, T&... o)
-{
- const std::string ext = serialization::extension(filename);
-
- if (ext == serialization::Extensions::BIN::value)
- {
- serialization::SerializerBin::load(filename, o...);
- }
- else if (ext == serialization::Extensions::XML::value)
- {
- serialization::SerializerXML::load(filename, o...);
- }
- else if (ext == serialization::Extensions::JSON::value)
- {
- serialization::SerializerJSON::load(filename, o...);
- }
- else if (ext == "") // falback is json
- {
- serialization::SerializerJSON::load(filename, o...);
- }
- else
- {
- throw std::runtime_error("Unknown file extension : " + filename);
- }
-}
-
-} /* namespace wolf */
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_IO_H_ */
diff --git a/serialization/cereal/serialization_eigen_core.h b/serialization/cereal/serialization_eigen_core.h
deleted file mode 100644
index 8fb5b9b0c..000000000
--- a/serialization/cereal/serialization_eigen_core.h
+++ /dev/null
@@ -1,125 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_H_
-#define _WOLF_IO_CEREAL_EIGEN_H_
-
-// Wolf includes
-#include <Eigen/Dense>
-#include <cereal/cereal.hpp>
-
-namespace cereal {
-
-/**
- * @brief Save Eigen::Matrix<...> to text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value,
-void>::type save(Archive& ar,
- const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows = mat.rows();
- decltype(mat.cols()) cols = mat.cols();
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-}
-
-/**
- * @brief Save Eigen::Matrix<...> to binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value,
-void>::type save(Archive& ar,
- const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows = mat.rows();
- decltype(mat.cols()) cols = mat.cols();
-
- ar(rows);
- ar(cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-/**
- * @brief Load compile-time sized Eigen::Matrix from text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- _Rows != Eigen::Dynamic and _Cols != Eigen::Dynamic,
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-}
-
-/**
- * @brief Load dynamic sized Eigen::Matrix from text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- (_Rows == Eigen::Dynamic or _Cols == Eigen::Dynamic),
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-
- //mat.resize(rows, cols);
-}
-
-/**
- * @brief Load compile-time sized Eigen::Matrix from binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- _Rows != Eigen::Dynamic and _Cols != Eigen::Dynamic,
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(rows);
- ar(cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-/**
- * @brief Load dynamic sized Eigen::Matrix from binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- (_Rows == Eigen::Dynamic or _Cols == Eigen::Dynamic),
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(rows);
- ar(cols);
-
- mat.resize(rows, cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_H_ */
diff --git a/serialization/cereal/serialization_eigen_geometry.h b/serialization/cereal/serialization_eigen_geometry.h
deleted file mode 100644
index f5d3ac61f..000000000
--- a/serialization/cereal/serialization_eigen_geometry.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-#define _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-
-// Wolf includes
-#include <Eigen/Geometry>
-
-#include "serialization_eigen_core.h"
-
-namespace cereal {
-
-template<class Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
-inline void save(Archive& ar,
- const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
-{
- save(ar, t.matrix());
-}
-
-template<class Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
-inline void load(Archive& ar,
- Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
-{
- load(ar, t.matrix());
-}
-
-template<class Archive, typename _Scalar>
-void serialize(Archive & ar,
- Eigen::Quaternion<_Scalar>& q,
- const std::uint32_t /*version*/)
-{
- ar(cereal::make_nvp("w", q.w()));
- ar(cereal::make_nvp("x", q.x()));
- ar(cereal::make_nvp("y", q.y()));
- ar(cereal::make_nvp("z", q.z()));
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_ */
diff --git a/serialization/cereal/serialization_eigen_sparse.h b/serialization/cereal/serialization_eigen_sparse.h
deleted file mode 100644
index 9039b6920..000000000
--- a/serialization/cereal/serialization_eigen_sparse.h
+++ /dev/null
@@ -1,79 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-#define _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-
-// Wolf includes
-#include <Eigen/Sparse>
-
-#include "serialization_eigen_core.h"
-#include <cereal/types/vector.hpp>
-
-namespace cereal {
-
-template<class Archive, typename Scalar, typename Index>
-inline void save(Archive& ar,
- const Eigen::Triplet<Scalar, Index>& t)
-{
- ar(cereal::make_nvp("row", t.row()));
- ar(cereal::make_nvp("col", t.col()));
- ar(cereal::make_nvp("value", t.value()));
-}
-
-template<class Archive, typename Scalar, typename Index>
-inline void load(Archive& ar,
- Eigen::Triplet<Scalar, Index>& t)
-{
- Index row, col;
- Scalar value;
-
- ar(cereal::make_nvp("row", row));
- ar(cereal::make_nvp("col", col));
- ar(cereal::make_nvp("value", value));
-
- t = Eigen::Triplet<Scalar, Index>(row, col, value);
-}
-
-template <class Archive, typename _Scalar, int _Options, typename _Index>
-void save(Archive& ar,
- const Eigen::SparseMatrix<_Scalar, _Options, _Index>& m)
-{
- _Index inner_size = m.innerSize();
- _Index outer_size = m.outerSize();
-
- using Triplet = Eigen::Triplet<_Scalar>;
- std::vector<Triplet> triplets;
-
- for (_Index i=0; i < outer_size; ++i)
- for (typename Eigen::SparseMatrix<_Scalar, _Options, _Index>::InnerIterator it(m,i); it; ++it)
- triplets.emplace_back( it.row(), it.col(), it.value() );
-
- ar(cereal::make_nvp("inner_size", inner_size));
- ar(cereal::make_nvp("outer_size", outer_size));
- ar(cereal::make_nvp("triplets", triplets));
-}
-
-template <class Archive, typename _Scalar, int _Options, typename _Index>
-void load(Archive& ar,
- Eigen::SparseMatrix<_Scalar, _Options, _Index>& m)
-{
- _Index inner_size;
- _Index outer_size;
-
- ar(cereal::make_nvp("inner_size", inner_size));
- ar(cereal::make_nvp("outer_size", outer_size));
-
- _Index rows = (m.IsRowMajor)? outer_size : inner_size;
- _Index cols = (m.IsRowMajor)? inner_size : outer_size;
-
- m.resize(rows, cols);
-
- using Triplet = Eigen::Triplet<_Scalar>;
- std::vector<Triplet> triplets;
-
- ar(cereal::make_nvp("triplets", triplets));
-
- m.setFromTriplets(triplets.begin(), triplets.end());
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
deleted file mode 100644
index b43c35673..000000000
--- a/serialization/cereal/serialization_local_parametrization_base.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-
-#include "core/state_block/local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-// Since classes deriving from LocalParametrizationBase
-// have default constructor calling the non-default
-// LocalParametrizationBase constructor with pre-defined
-// arguments, there is nothing to save here.
-template<class Archive>
-inline void serialize(
- Archive& /*ar*/,
- wolf::LocalParametrizationBase& /*lpb*/,
- std::uint32_t const /*file_version*/)
-{
- //
-}
-
-} //namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_homogeneous.h b/serialization/cereal/serialization_local_parametrization_homogeneous.h
deleted file mode 100644
index 9fcc656d5..000000000
--- a/serialization/cereal/serialization_local_parametrization_homogeneous.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-
-#include "core/local_parametrization_homogeneous.h"
-
-#include "serialization_local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationHomogeneous,
- "LocalParametrizationHomogeneous");
-
-namespace cereal {
-
-template<class Archive>
-inline void serialize(
- Archive& ar,
- wolf::LocalParametrizationHomogeneous& lp,
- std::uint32_t const /*file_version*/)
-{
- ar( cereal::make_nvp("LocalParametrizationBase",
- cereal::base_class<wolf::LocalParametrizationBase>(&lp)) );
-}
-
-} //namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_quaternion.h b/serialization/cereal/serialization_local_parametrization_quaternion.h
deleted file mode 100644
index 66fe30a36..000000000
--- a/serialization/cereal/serialization_local_parametrization_quaternion.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-#define WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-
-#include "core/local_parametrization_quaternion.h"
-
-#include "serialization_local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationQuaternion<wolf::DQ_LOCAL>,
- "wolf_LocalParametrizationQuaternion_DQ_LOCAL")
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationQuaternion<wolf::DQ_GLOBAL>,
- "wolf_LocalParametrizationQuaternion_DQ_GLOBAL")
-
-namespace cereal {
-
-template<class Archive, unsigned int DeltaReference>
-inline void serialize(
- Archive &ar,
- wolf::LocalParametrizationQuaternion<DeltaReference> &lp,
- const unsigned int /*file_version*/)
-{
- ar( cereal::make_nvp("LocalParametrizationBase",
- cereal::base_class<wolf::LocalParametrizationBase>(&lp)) );
-}
-
-} //namespace boost
-
-#endif /* WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_ */
diff --git a/serialization/cereal/serialization_node_base.h b/serialization/cereal/serialization_node_base.h
deleted file mode 100644
index a2c592d69..000000000
--- a/serialization/cereal/serialization_node_base.h
+++ /dev/null
@@ -1,77 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_NODE_BASE_H_
-#define _WOLF_IO_CEREAL_NODE_BASE_H_
-
-// Wolf includes
-#include "core/node_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace wolf {
-
-struct NodeBase::Serializer {
-
- template <class Archive>
- static void serialize(Archive& ar, NodeBase& o, std::uint32_t const /*version*/)
- {
- ar( cereal::make_nvp("node_class_", o.node_category_) );
- ar( cereal::make_nvp("node_type_", o.node_type_) );
- ar( cereal::make_nvp("node_name_", o.node_name_) );
- ar( cereal::make_nvp("node_id_", o.node_id_) );
-
-// ar( cereal::make_nvp("problem_ptr_", o.problem_ptr_) );
-
- // Not sure what to do with this guy ...
- //ar( cereal::make_nvp("node_id_count_", o.node_id_count_) );
- }
-
- template <class Archive>
- static void load_and_construct( Archive& ar, cereal::construct<wolf::NodeBase>& construct,
- std::uint32_t const /*version*/ )
- {
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_class;
- decltype(std::declval<wolf::NodeBase>().getType()) nb_type;
- decltype(std::declval<wolf::NodeBase>().getName()) nb_name;
-
- ar( cereal::make_nvp("node_class_", nb_class) );
- ar( cereal::make_nvp("node_type_", nb_type) );
- ar( cereal::make_nvp("node_name_", nb_name) );
-
- construct( nb_class, nb_type, nb_name );
-
- ar( cereal::make_nvp("node_id_", construct->node_id_) );
-
-// ar( cereal::make_nvp("problem_ptr_", construct->problem_ptr_) );
-
- // Not sure what to do with this guy ...
- //ar( cereal::make_nvp("node_id_count_", construct->node_id_count_) );
- }
-};
-
-} // namespace wolf
-
-namespace cereal {
-
-/// @note No default constructor thus the need
-/// for these specializations
-template <>
-struct LoadAndConstruct<wolf::NodeBase>
-{
- template <class Archive>
- static void load_and_construct( Archive& ar,
- cereal::construct<wolf::NodeBase>& construct,
- std::uint32_t const version )
- {
- wolf::NodeBase::Serializer::load_and_construct(ar, construct, version);
- }
-};
-
-template <class Archive>
-void serialize(Archive& ar, wolf::NodeBase& o, std::uint32_t const version)
-{
- wolf::NodeBase::Serializer::serialize(ar, o, version);
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_NODE_BASE_H_ */
diff --git a/serialization/cereal/serialization_processor_odom2d_params.h b/serialization/cereal/serialization_processor_odom2d_params.h
deleted file mode 100644
index dc0416b94..000000000
--- a/serialization/cereal/serialization_processor_odom2d_params.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
-
-// Wolf includes
-#include "core/processor/processor_odom_2D.h"
-#include "serialization_processor_params_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsOdom2D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("ProcessorParamsBase",
- cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
-
- ar( cereal::make_nvp("cov_det_th_", o.cov_det) );
- ar( cereal::make_nvp("dist_traveled_th_", o.dist_traveled_th_) );
- ar( cereal::make_nvp("elapsed_time_th_", o.elapsed_time_th_) );
- ar( cereal::make_nvp("theta_traveled_th_", o.theta_traveled_th_) );
- ar( cereal::make_nvp("unmeasured_perturbation_std_",
- o.unmeasured_perturbation_std) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::ProcessorParamsOdom2D, "ProcessorParamsOdom2D")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ */
diff --git a/serialization/cereal/serialization_processor_odom3d_params.h b/serialization/cereal/serialization_processor_odom3d_params.h
deleted file mode 100644
index d2fd7c077..000000000
--- a/serialization/cereal/serialization_processor_odom3d_params.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
-
-// Wolf includes
-#include "core/processor/processor_odom_3D.h"
-#include "serialization_processor_params_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsOdom3D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("ProcessorParamsBase",
- cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
-
- ar( cereal::make_nvp("angle_turned", o.angle_turned) );
- ar( cereal::make_nvp("dist_traveled", o.dist_traveled) );
- ar( cereal::make_nvp("max_buff_length", o.max_buff_length) );
- ar( cereal::make_nvp("max_time_span", o.max_time_span) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::ProcessorParamsOdom3D, "ProcessorOdom3DParams")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ */
diff --git a/serialization/cereal/serialization_processor_params_base.h b/serialization/cereal/serialization_processor_params_base.h
deleted file mode 100644
index 03ea158c0..000000000
--- a/serialization/cereal/serialization_processor_params_base.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
-
-// Wolf includes
-#include "core/processor/processor_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsBase& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("type", o.type) );
- ar( cereal::make_nvp("name", o.name) );
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ */
diff --git a/serialization/cereal/serialization_sensor_intrinsic_base.h b/serialization/cereal/serialization_sensor_intrinsic_base.h
deleted file mode 100644
index 86b2a9b48..000000000
--- a/serialization/cereal/serialization_sensor_intrinsic_base.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
-#define _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
-
-// Wolf includes
-#include "core/sensor/sensor_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::IntrinsicsBase& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("type", o.type) );
- ar( cereal::make_nvp("name", o.name) );
-}
-
-} // namespace cereal
-
-// No need to register base
-//CEREAL_REGISTER_TYPE_WITH_NAME(wolf::IntrinsicsBase, "IntrinsicsBase");
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ */
diff --git a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
deleted file mode 100644
index 17d4160b3..000000000
--- a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
-#define _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
-
-// Wolf includes
-#include "core/sensor/sensor_odom_2D.h"
-
-#include "serialization_sensor_intrinsic_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::IntrinsicsOdom2D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("IntrinsicsBase",
- cereal::base_class<wolf::IntrinsicsBase>(&o)) );
-
- ar( cereal::make_nvp("k_disp_to_disp", o.k_disp_to_disp) );
- ar( cereal::make_nvp("k_rot_to_rot", o.k_rot_to_rot) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::IntrinsicsOdom2D, "IntrinsicsOdom2D")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_ */
diff --git a/serialization/cereal/serialization_time_stamp.h b/serialization/cereal/serialization_time_stamp.h
deleted file mode 100644
index f0c978d36..000000000
--- a/serialization/cereal/serialization_time_stamp.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_TIME_STAMP_H_
-#define _WOLF_IO_CEREAL_TIME_STAMP_H_
-
-// Wolf includes
-#include "core/time_stamp.h"
-
-#include <cereal/cereal.hpp>
-
-namespace cereal {
-
-/// @note serialization versionning raise
-/// a compile error here...
-template <class Archive>
-void save(Archive& ar, const wolf::TimeStamp& o/*, std::uint32_t const version*/)
-{
- ar( cereal::make_nvp("value", o.get()) );
-}
-
-template <class Archive>
-void load(Archive& ar, wolf::TimeStamp& o/*, std::uint32_t const version*/)
-{
- auto val = o.get();
-
- ar( cereal::make_nvp("value", val) );
-
- o.set(val);
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_TIME_STAMP_H_ */
diff --git a/test/serialization/CMakeLists.txt b/test/serialization/CMakeLists.txt
deleted file mode 100644
index 6902132bd..000000000
--- a/test/serialization/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-# cereal
-IF(cereal_FOUND)
- add_subdirectory(cereal)
-ENDIF(cereal_FOUND)
diff --git a/test/serialization/cereal/CMakeLists.txt b/test/serialization/cereal/CMakeLists.txt
deleted file mode 100644
index 733a36e25..000000000
--- a/test/serialization/cereal/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-# NodeBase serialization class test
-wolf_add_gtest(gtest_cereal_serialization_node_base gtest_serialization_node_base.cpp)
-target_link_libraries(gtest_cereal_serialization_node_base ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_local_parametrization
- gtest_serialization_local_parametrization.cpp)
-target_link_libraries(gtest_cereal_serialization_local_parametrization ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_sensor_intrinsic_base
- gtest_serialization_sensor_intrinsic_base.cpp)
-target_link_libraries(gtest_cereal_serialization_sensor_intrinsic_base ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_sensor_odom2d_intrinsic
- gtest_serialization_sensor_odom2d_intrinsic.cpp)
-target_link_libraries(gtest_cereal_serialization_sensor_odom2d_intrinsic ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_save_load
- gtest_serialization_save_load.cpp)
-target_link_libraries(gtest_cereal_serialization_save_load ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_processor_odom3d_params
- gtest_serialization_processor_odom3d_params.cpp)
-target_link_libraries(gtest_cereal_serialization_processor_odom3d_params ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_processor_odom2d_params
- gtest_serialization_processor_odom2d_params.cpp)
-target_link_libraries(gtest_cereal_serialization_processor_odom2d_params ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_time_stamp
- gtest_serialization_time_stamp.cpp)
-target_link_libraries(gtest_cereal_serialization_time_stamp ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_core
- gtest_serialization_eigen_core.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_core ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_geometry
- gtest_serialization_eigen_geometry.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_geometry ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_sparse
- gtest_serialization_eigen_sparse.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_sparse ${PROJECT_NAME})
diff --git a/test/serialization/cereal/gtest_serialization_eigen_core.cpp b/test/serialization/cereal/gtest_serialization_eigen_core.cpp
deleted file mode 100644
index 28859dd7a..000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_core.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_core.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationEigen : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationEigen()
- {
- nb_ = f_mat_t::Random();
-
- dnb_ = d_mat_t::Random(10, 10);
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_eigen";
-
- const std::vector<std::string> exts = {".bin"/*, ".xml", ".json"*/};
-
- using f_mat_t = Eigen::Matrix<double, 5, 5>;
-
- using d_mat_t = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
-
- f_mat_t nb_;
-
- d_mat_t dnb_;
-};
-
-TEST_F(WolfTestCerealSerializationEigen,
- CerealSerializationEigenFixedMat)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- WolfTestCerealSerializationEigen::f_mat_t nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- EXPECT_EQ(nb_load, nb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationEigen::"
- "CerealSerializationEigenFixedMat !\n");
-}
-
-TEST_F(WolfTestCerealSerializationEigen,
- CerealSerializationEigenDynamicMat)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, dnb_ ) )
- << "Failed on saving " << full_path;
-
- WolfTestCerealSerializationEigen::d_mat_t dnb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, dnb_load ) )
- << "Failed on loading " << full_path;
-
- EXPECT_EQ(dnb_load, dnb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationEigen::"
- "CerealSerializationEigenDynamicMat !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
deleted file mode 100644
index 87cafb4d5..000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_geometry.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationEigenGeo : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationEigenGeo()
- {
- iso_2d_ = Eigen::Isometry2d(Eigen::Rotation2Dd(0.17));
- iso_2d_.translation() << 0.5, 1.8;
-
- q_d_ = Eigen::Vector4d().setRandom().normalized();
-
- iso_3d_ = Eigen::Isometry3d(q_d_);
- iso_3d_.translation() << -7.245, +3.88, 0.0001;
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_eigen_geo";
-
- const std::vector<std::string> exts = {".bin"/*, ".xml", ".json"*/};
-
- Eigen::Isometry2d iso_2d_;
- Eigen::Isometry3d iso_3d_;
- Eigen::Quaterniond q_d_;
-};
-
-TEST_F(WolfTestCerealSerializationEigenGeo,
- CerealSerializationEigenIso2d)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, iso_2d_, iso_3d_, q_d_) )
- << "Failed on saving " << full_path;
-
- Eigen::Isometry2d iso_2d_loaded;
- Eigen::Isometry3d iso_3d_loaded;
- Eigen::Quaterniond q_d_loaded;
-
- ASSERT_NO_THROW( wolf::load( full_path, iso_2d_loaded, iso_3d_loaded, q_d_loaded) )
- << "Failed on loading " << full_path;
-
- ASSERT_MATRIX_APPROX(iso_2d_.matrix(), iso_2d_loaded.matrix(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(iso_3d_.matrix(), iso_3d_loaded.matrix(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(q_d_.coeffs(), q_d_loaded.coeffs(), wolf::Constants::EPS);
- }
-
- PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
deleted file mode 100644
index 0b803afdd..000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_sparse.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <fstream>
-
-class WolfTestCerealSerializationEigenSparse : public testing::Test
-{
-public:
-
- using triplet_t = Eigen::Triplet<double>;
- using sparse_mat_t = Eigen::SparseMatrix<double>;
-
- WolfTestCerealSerializationEigenSparse()
- {
- triplet_list_.reserve(10);
-
- for(int i=0; i<10; ++i)
- triplet_list_.emplace_back(i,i,i*5);
-
- m_.resize(10, 10);
- m_.setFromTriplets(triplet_list_.begin(), triplet_list_.end());
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename_t = "serialization_eigen_triplet";
- const std::string filename_m = "serialization_eigen_sparse";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- triplet_t t_ = Eigen::Triplet<double>(1, 2, 5.5);
-
- std::vector<triplet_t> triplet_list_;
- Eigen::SparseMatrix<double> m_;
-};
-
-TEST_F(WolfTestCerealSerializationEigenSparse,
- CerealSerializationEigenTriplet)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename_t + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, t_) )
- << "Failed on saving " << full_path;
-
- triplet_t t;
-
- ASSERT_NO_THROW( wolf::load( full_path, t) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(t_.row(), t.row());
- ASSERT_EQ(t_.col(), t.col());
- ASSERT_EQ(t_.value(), t.value());
- }
-
- PRINT_TEST_FINISHED;
-}
-
-TEST_F(WolfTestCerealSerializationEigenSparse,
- CerealSerializationEigenSparseMatrix)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename_m + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, m_) )
- << "Failed on saving " << full_path;
-
- sparse_mat_t m;
-
- ASSERT_NO_THROW( wolf::load( full_path, m) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(m_.rows(), m.rows());
- ASSERT_EQ(m_.cols(), m.cols());
-
- std::vector<triplet_t> triplet_list;
- triplet_list.reserve(10);
-
- for (int k=0; k<m.outerSize(); ++k)
- for (sparse_mat_t::InnerIterator it(m, k); it; ++it)
- {
- triplet_list.emplace_back(it.row(), it.col(), it.value());
- }
-
- ASSERT_EQ(triplet_list_.size(), triplet_list.size());
-
- for (int i=0; i<triplet_list_.size(); ++i)
- {
- ASSERT_EQ(triplet_list_[i].row(), triplet_list[i].row());
- ASSERT_EQ(triplet_list_[i].col(), triplet_list[i].col());
- ASSERT_EQ(triplet_list_[i].value(), triplet_list[i].value());
- }
- }
-
- PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
deleted file mode 100644
index 3df34a27a..000000000
--- a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
+++ /dev/null
@@ -1,571 +0,0 @@
-/*
- * gtest_node_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_local_parametrization_quaternion.h"
-#include "../../../serialization/cereal/serialization_local_parametrization_homogeneous.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-///////////////////////////////////////
-/// LocalParametrizationHomogeneous ///
-///////////////////////////////////////
-
-const std::string path_to_io = "/tmp/";
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrBIN !\n");
-}
-
-//////////////////////////////////////
-/// LocalParametrizationQuaternion ///
-//////////////////////////////////////
-
-//////////////////////////////////////
-/// LOCAL ///
-//////////////////////////////////////
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrBIN !\n");
-}
-
-//////////////////////////////////////
-/// GLOBAL ///
-//////////////////////////////////////
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrBIN !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_node_base.cpp b/test/serialization/cereal/gtest_serialization_node_base.cpp
deleted file mode 100644
index 8b6140fe7..000000000
--- a/test/serialization/cereal/gtest_serialization_node_base.cpp
+++ /dev/null
@@ -1,247 +0,0 @@
-/*
- * gtest_node_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_node_base.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationNodeBase : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationNodeBase() /*:
- nb(nb_class),
- nb_ptr(std::make_shared<wolf::NodeBase>(nb_class))*/
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_class = "Foo";
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_type = "Bar";
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_name = "Dummy";
-
- decltype(std::declval<wolf::NodeBase>().nodeId()) id;
-
-// wolf::NodeBase nb;
-// wolf::NodeBasePtr nb_ptr;
-};
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseXML)
-{
- {
- // This guy has node_id = 1
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- // This guy has node_id = 2
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrXML)
-{
- {
- // This guy has node_id = 3
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseJSON)
-{
- {
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::NodeBase blank("This guy has node_id = 1");
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrJSON)
-{
- {
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseBinary)
-{
- {
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::NodeBase blank("This guy has node_id = 1");
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrBinary)
-{
- {
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp b/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
deleted file mode 100644
index 89fbef3d6..000000000
--- a/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_processor_odom2d_params.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationProcessorParamsOdom2D : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationProcessorParamsOdom2D()
- {
- nb_.name = "NAME";
- nb_.type = "ODOM 2D";
-
- nb_.dist_traveled_th_ = 0.17;
- nb_.theta_traveled_th_ = 0.3;
- nb_.cov_det = 0.4;
- nb_.elapsed_time_th_ = 1.5;
- nb_.unmeasured_perturbation_std = 1e-5;
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_processor_odom2d_params";
- const std::string ptr_ext = "_ptr";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- wolf::ProcessorParamsOdom2D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationProcessorParamsOdom2D,
- CerealSerializationProcessorParamsOdom2D)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- wolf::ProcessorParamsOdom2D nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(nb_load.type, nb_.type) << full_path;
- ASSERT_EQ(nb_load.name, nb_.name) << full_path;
- ASSERT_EQ(nb_load.dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.cov_det, nb_.cov_det) << full_path;
- ASSERT_EQ(nb_load.unmeasured_perturbation_std,
- nb_.unmeasured_perturbation_std) << full_path;
-
- /// Testing BasePtr
-
- const std::string ptr_full_path = path_to_io + filename + ptr_ext + ext;
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom2D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( ptr_full_path, nb ) )
- << "Failed on saving " << ptr_full_path;
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( ptr_full_path, nb ) )
- << "Failed on loading " << ptr_full_path;
-
- wolf::ProcessorParamsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr)
- << "Failed on casting " << ptr_full_path;
-
- ASSERT_EQ(nb_cast->type, nb_.type) << full_path;
- ASSERT_EQ(nb_cast->name, nb_.name) << full_path;
- ASSERT_EQ(nb_cast->dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->cov_det, nb_.cov_det) << full_path;
- ASSERT_EQ(nb_cast->unmeasured_perturbation_std,
- nb_.unmeasured_perturbation_std) << full_path;
- }
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorParamsOdom2D::"
- "CerealSerializationProcessorParamsOdom2D !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp b/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
deleted file mode 100644
index 41b2ca2f1..000000000
--- a/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_processor_odom3d_params.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationProcessorOdom3DParams : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationProcessorOdom3DParams()
- {
- nb_.name = "NAME";
- //nb_.type = "ODOM 3D";
-
- nb_.max_time_span = 1.5;
- nb_.max_buff_length = 55.;
- nb_.dist_traveled = .25;
- nb_.angle_turned = .17;
- }
-
- const std::string path_to_io = "/tmp/";
-
- wolf::ProcessorParamsOdom3D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsXML)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.xml");
-
- wolf::ProcessorParamsOdom3D nb_save;
- nb_save.name = "NAME2";
- //nb_.type = "ODOM 3D";
-
- nb_save.max_time_span = 2.5;
- nb_save.max_buff_length = 52.;
- nb_save.dist_traveled = .24;
- nb_save.angle_turned = .18;
-
- ASSERT_NO_THROW( wolf::save( filename, nb_, nb_save, 10 ) );
-
- {
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- wolf::ProcessorParamsOdom3D nb_load0, nb_load1;
- int myint;
- ASSERT_NO_THROW( wolf::load( filename, nb_load0, nb_load1, myint ) );
-
- ASSERT_EQ(nb_load0.type, nb_.type);
- ASSERT_EQ(nb_load0.name, nb_.name);
- ASSERT_EQ(nb_load0.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load0.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load0.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load0.angle_turned, nb_.angle_turned);
-
- ASSERT_EQ(nb_load1.type, nb_save.type);
- ASSERT_EQ(nb_load1.name, nb_save.name);
- ASSERT_EQ(nb_load1.max_time_span, nb_save.max_time_span);
- ASSERT_EQ(nb_load1.max_buff_length, nb_save.max_buff_length);
- ASSERT_EQ(nb_load1.dist_traveled, nb_save.dist_traveled);
- ASSERT_EQ(nb_load1.angle_turned, nb_save.angle_turned);
-
- ASSERT_EQ(myint, 10);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrXML)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.xml");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsJSON)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.json");
-
- ASSERT_NO_THROW( wolf::save( filename, nb_ ) );
-
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrJSON)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.json");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsBinary)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.bin");
-
- ASSERT_NO_THROW( wolf::save( filename, nb_ ) );
-
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrBinary)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.bin");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_save_load.cpp b/test/serialization/cereal/gtest_serialization_save_load.cpp
deleted file mode 100644
index 23241af48..000000000
--- a/test/serialization/cereal/gtest_serialization_save_load.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/io.h"
-#include "../../../serialization/cereal/serialization_sensor_odom2d_intrinsic.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-namespace wolf {
-
-using IntrinsicsOdom2DPtr = std::shared_ptr<IntrinsicsOdom2D>;
-
-}
-
-class WolfTestCerealSerializationSaveLoad : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationSaveLoad()
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::IntrinsicsOdom2D>().type) nb_type = "TYPE";
- decltype(std::declval<wolf::IntrinsicsOdom2D>().name) nb_name = "NAME";
- decltype(std::declval<wolf::IntrinsicsOdom2D>().k_disp_to_disp) nb_k_disp_to_disp = 0.54;
- decltype(std::declval<wolf::IntrinsicsOdom2D>().k_rot_to_rot) nb_k_rot_to_rot = 0.18;
-};
-
-TEST_F(WolfTestCerealSerializationSaveLoad, CerealSerializationSaveLoadExtension)
-{
- const std::string xml = "/test/filename.xml";
- const std::string bin = "/test/filename.bin";
- const std::string json = "/test/filename.json";
-
- ASSERT_EQ(wolf::serialization::extension(xml), ".xml");
- ASSERT_EQ(wolf::serialization::extension(bin), ".bin");
- ASSERT_EQ(wolf::serialization::extension(json), ".json");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadXML)
-{
- const std::string filename = path_to_io + "save_load_serialization.xml";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadJSON)
-{
- const std::string filename = path_to_io + "save_load_serialization.json";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadBinary)
-{
- const std::string filename = path_to_io + "save_load_serialization.bin";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadNoExt)
-{
- const std::string filename = path_to_io + "save_load_serialization_no_ext";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadNoExt !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadUnknownExt)
-{
- const std::string filename = path_to_io + "save_load_serialization.foo";
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_THROW( wolf::save( filename, nb ), std::runtime_error );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_THROW( wolf::load( filename, nb ), std::runtime_error );
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadUnknownExt !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp b/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
deleted file mode 100644
index a8a67dc19..000000000
--- a/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * gtest_intrinsics_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_sensor_intrinsic_base.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationIntrinsicsBase : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationIntrinsicsBase()
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::IntrinsicsBase>().type) nb_type = "TYPE";
- decltype(std::declval<wolf::IntrinsicsBase>().name) nb_name = "NAME";
-};
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseXML)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBasePtrXML)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseJSON)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBasePtrJSON)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseBinary)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase, CerealSerializationIntrinsicsBasePtrBinary)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp b/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
deleted file mode 100644
index c5891c3d9..000000000
--- a/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
+++ /dev/null
@@ -1,256 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_sensor_odom2d_intrinsic.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-namespace wolf {
-
-using IntrinsicsOdom2DPtr = std::shared_ptr<IntrinsicsOdom2D>;
-
-}
-
-class WolfTestCerealSerializationIntrinsicsOdom2D : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationIntrinsicsOdom2D()
- {
- nb_.k_disp_to_disp = 0.54;
- nb_.k_rot_to_rot = 0.18;
- nb_.name = "NAME";
- nb_.type = "TYPE";
- }
-
- const std::string path_to_io = "/tmp/";
-
- wolf::IntrinsicsOdom2D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DXML)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DPtrXML)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DJSON)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DPtrJSON)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DBinary)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D, CerealSerializationIntrinsicsOdom2DPtrBinary)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_time_stamp.cpp b/test/serialization/cereal/gtest_serialization_time_stamp.cpp
deleted file mode 100644
index 8dc4cd479..000000000
--- a/test/serialization/cereal/gtest_serialization_time_stamp.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_time_stamp.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationTimeStamp : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationTimeStamp()
- {
- nb_.setToNow();
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_time_stamp";
- const std::string ptr_ext = "_ptr";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- wolf::TimeStamp nb_;
-};
-
-TEST_F(WolfTestCerealSerializationTimeStamp,
- CerealSerializationTimeStamp)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- wolf::TimeStamp nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(nb_load, nb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationTimeStamp::"
- "CerealSerializationTimeStamp !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
--
GitLab