Commit 9751edb6 authored by mederic_fourmy's avatar mederic_fourmy
Browse files

[skip-ci] Merge branch 'devel' into 25-follow-core-465-migrate-from-stateblock-to-statederived

parents 919c1940 bc67109c
......@@ -75,7 +75,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============
FIND_PACKAGE(wolfcore REQUIRED CONFIG)
FIND_PACKAGE(wolfvision REQUIRED)
FIND_PACKAGE(apriltag REQUIRED)
FIND_PACKAGE(apriltag REQUIRED) # UMichigan apriltag library
#============ CONFIG.H ============
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
......@@ -101,10 +101,12 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
# ============ HEADERS ============
SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_apriltag.h
include/${PROJECT_NAME}/factor/factor_apriltag_proj.h
)
SET(HDRS_FEATURE
include/${PROJECT_NAME}/feature/feature_apriltag.h
include/${PROJECT_NAME}/feature/feature_apriltag_pose.h
include/${PROJECT_NAME}/feature/feature_apriltag_proj.h
)
SET(HDRS_LANDMARK
include/${PROJECT_NAME}/landmark/landmark_apriltag.h
......@@ -116,7 +118,6 @@ include/${PROJECT_NAME}/processor/ippe.h
# ============ SOURCES ============
SET(SRCS_FEATURE
src/feature/feature_apriltag.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_apriltag.cpp
......
......@@ -34,7 +34,7 @@
#include "core/sensor/sensor_camera.h"
#include "core/processor/processor_tracker_landmark_apriltag.h"
#include "core/capture/capture_image.h"
#include "core/feature/feature_apriltag.h"
#include "core/feature/feature_apriltag_pose.h"
// opencv
#include <opencv2/imgproc/imgproc.hpp>
......@@ -179,7 +179,7 @@ int main(int argc, char *argv[])
auto img = std::static_pointer_cast<CaptureImage>(cap);
for (FeatureBasePtr f : img->getFeatureList())
{
FeatureApriltagPtr fa = std::static_pointer_cast<FeatureApriltag>(f);
FeatureApriltagPosePtr fa = std::static_pointer_cast<FeatureApriltagPose>(f);
draw_apriltag(img->getImage(), fa->getTagCorners(), 1);
}
cv::imshow( img->getName(), img->getImage() ); // display original image.
......
......@@ -26,7 +26,9 @@ noise:
std_z : 0.1 # m
std_rpy_degrees : 5 # degrees
std_pix: 20 # pixel error
use_proj_factor: false
vote:
voting active: true
min_time_vote: 0 # s
......@@ -34,8 +36,6 @@ vote:
min_features_for_keyframe: 12
nb_vote_for_every_first: 50
reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor
add_3d_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only
max_new_features: -1
apply_loss_function: true
......
......@@ -19,40 +19,46 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef _FACTOR_APRILTAG_H_
#define _FACTOR_APRILTAG_H_
//Wolf includes
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/factor/factor_autodiff.h"
#include "core/sensor/sensor_base.h"
#ifndef _FACTOR_APRILTAG_PROJ_H_
#define _FACTOR_APRILTAG_PROJ_H_
// Wolf apriltag
#include "apriltag/landmark/landmark_apriltag.h"
#include "apriltag/feature/feature_apriltag.h"
#include "apriltag/feature/feature_apriltag_proj.h"
// Wolf vision
#include <vision/math/pinhole_tools.h>
#include <vision/sensor/sensor_camera.h>
//Wolf core
#include <core/common/wolf.h>
#include <core/math/rotations.h>
#include <core/factor/factor_autodiff.h>
#include <core/sensor/sensor_base.h>
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorApriltag);
WOLF_PTR_TYPEDEFS(FactorApriltagProj);
class FactorApriltag : public FactorAutodiff<FactorApriltag, 6, 3, 4, 3, 4, 3, 4>
class FactorApriltagProj : public FactorAutodiff<FactorApriltagProj, 8, 3, 4, 3, 4, 3, 4>
{
public:
/** \brief Class constructor
*/
FactorApriltag(
FactorApriltagProj(
const FeatureApriltagProjPtr& _feature_ptr,
const SensorBasePtr& _sensor_ptr,
const FrameBasePtr& _frame_ptr,
const LandmarkApriltagPtr& _landmark_other_ptr,
const FeatureApriltagPtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status);
FactorStatus _status = FAC_ACTIVE);
/** \brief Class Destructor
*/
~FactorApriltag() override;
~FactorApriltagProj() override;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
......@@ -65,7 +71,7 @@ class FactorApriltag : public FactorAutodiff<FactorApriltag, 6, 3, 4, 3, 4, 3, 4
const T* const _o_landmark,
T* _residuals) const;
Eigen::Vector6d residual() const;
Eigen::Vector8d residual() const;
double cost() const;
// print function only for double (not Jet)
......@@ -80,6 +86,19 @@ class FactorApriltag : public FactorAutodiff<FactorApriltag, 6, 3, 4, 3, 4, 3, 4
// double prints stuff
WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
}
template<typename D1>
static Eigen::Matrix<typename D1::Scalar, 2, 1> pinholeProj(const Eigen::Matrix3d& K,
const Eigen::MatrixBase<D1>& p_c_l,
const Eigen::Quaternion<typename D1::Scalar>& q_c_l,
const Eigen::Vector3d& l_corn);
private:
Eigen::Vector3d l_corn1_;
Eigen::Vector3d l_corn2_;
Eigen::Vector3d l_corn3_;
Eigen::Vector3d l_corn4_;
SensorCameraConstPtr camera_;
Matrix3d K_;
};
} // namespace wolf
......@@ -90,15 +109,15 @@ class FactorApriltag : public FactorAutodiff<FactorApriltag, 6, 3, 4, 3, 4, 3, 4
namespace wolf
{
FactorApriltag::FactorApriltag(
inline FactorApriltagProj::FactorApriltagProj(
const FeatureApriltagProjPtr& _feature_ptr,
const SensorBasePtr& _sensor_ptr,
const FrameBasePtr& _frame_ptr,
const LandmarkApriltagPtr& _landmark_other_ptr,
const FeatureApriltagPtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorApriltag",
FactorAutodiff("FactorApriltagProj",
TOP_LMK,
_feature_ptr,
nullptr,
......@@ -113,18 +132,54 @@ FactorApriltag::FactorApriltag(
_landmark_other_ptr->getP(), _landmark_other_ptr->getO()
)
{
double tag_width = _feature_ptr->getTagWidth();
// Same order as the 2d corners (anti clockwise, looking at the tag).
// Looking at the tag, the reference frame is
// X = Right, Y = Down, Z = Inside the plane
double s = tag_width/2;
l_corn1_ << -s, s, 0; // bottom left
l_corn2_ << s, s, 0; // bottom right
l_corn3_ << s, -s, 0; // top right
l_corn4_ << -s, -s, 0; // top left
//////////////////////////////////////
// Camera matrix
K_ = std::static_pointer_cast<SensorCamera>(_sensor_ptr)->getIntrinsicMatrix();
}
/** \brief Class Destructor
*/
FactorApriltag::~FactorApriltag()
inline FactorApriltagProj::~FactorApriltagProj()
{
//
}
template<typename T> bool FactorApriltag::operator ()( const T* const _p_camera, const T* const _o_camera, const T* const _p_keyframe, const T* const _o_keyframe, const T* const _p_landmark, const T* const _o_landmark, T* _residuals) const
template<typename D1>
Eigen::Matrix<typename D1::Scalar, 2, 1> FactorApriltagProj::pinholeProj(const Eigen::Matrix3d& K,
const Eigen::MatrixBase<D1>& p_c_l,
const Eigen::Quaternion<typename D1::Scalar>& q_c_l,
const Eigen::Vector3d& l_corn)
{
MatrixSizeCheck<3,1>::check(p_c_l);
typedef typename D1::Scalar T;
Eigen::Matrix<T, 3, 1> h = K.cast<T>() * (p_c_l + q_c_l * l_corn.cast<T>());
Eigen::Matrix<T, 2, 1> pix; pix << h(0)/h(2), h(1)/h(2);
return pix;
}
template<typename T>
bool FactorApriltagProj::operator ()(const T* const _p_camera,
const T* const _o_camera,
const T* const _p_keyframe,
const T* const _o_keyframe,
const T* const _p_landmark,
const T* const _o_landmark,
T* _residuals) const
{
// Maps
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_camera);
......@@ -133,34 +188,31 @@ template<typename T> bool FactorApriltag::operator ()( const T* const _p_camera,
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_keyframe);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_landmark);
Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_landmark);
Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
Eigen::Map<Eigen::Matrix<T,8,1>> residuals(_residuals);
// Expected relative camera-lmk transformation
// Expected measurement
Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate();
Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l;
Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l);
// Measurement
Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>());
Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 );
Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
//Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
//////////////////////////////////////
// Expected corner projections
Eigen::Matrix<T, 8, 1> corners_exp;
corners_exp.segment(0,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn1_);
corners_exp.segment(2,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn2_);
corners_exp.segment(4,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn3_);
corners_exp.segment(6,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn4_);
// Error
Eigen::Matrix<T, 6, 1> err;
err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l);
//err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l);
err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec();
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (corners_exp - getMeasurement().cast<T>());
return true;
}
Eigen::Vector6d FactorApriltag::residual() const
inline Eigen::Vector8d FactorApriltagProj::residual() const
{
Eigen::Vector6d res;
Eigen::Vector8d res;
double * p_camera, * o_camera, * p_frame, * o_frame, * p_tag, * o_tag;
p_camera = getCapture()->getSensorP()->getState().data();
o_camera = getCapture()->getSensorO()->getState().data();
......@@ -174,11 +226,12 @@ Eigen::Vector6d FactorApriltag::residual() const
return res;
}
double FactorApriltag::cost() const
inline double FactorApriltagProj::cost() const
{
return residual().squaredNorm();
}
} // namespace wolf
#endif /* _CONSTRAINT_AUTODIFF_APRILTAG_H_ */
#endif /* _FACTOR_APRILTAG_PROJ_H_ */
......@@ -25,12 +25,6 @@
//Wolf includes
#include "core/feature/feature_base.h"
//std includes
//external library incudes
#include "apriltag/apriltag.h"
#include "apriltag/common/zarray.h"
// opencv
#include <opencv2/features2d.hpp>
......@@ -38,47 +32,75 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureApriltag);
//class FeatureApriltag
class FeatureApriltag : public FeatureBase
{
public:
FeatureApriltag(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_covariance,
const int _tag_id,
const apriltag_detection_t & _det,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
FeatureApriltag(
const std::string _type,
const Eigen::VectorXd & _measurement,
const Eigen::MatrixXd & _meas_covariance,
const int _tag_id,
const Vector8d & _corners_vec,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
~FeatureApriltag() override;
/** \brief Returns tag id
*
* Returns tag id
/** \brief Return tag id
*
**/
double getTagId() const;
const apriltag_detection_t& getDetection() const;
int getTagId() const;
/** \brief Return vector of tag corners
*
**/
const std::vector<cv::Point2d>& getTagCorners() const;
double getRepError1() const;
double getRepError2() const;
bool getUserotation() const;
private:
int tag_id_;
std::vector<cv::Point2d> tag_corners_;
apriltag_detection_t detection_;
double rep_error1_;
double rep_error2_;
bool use_rotation_;
};
inline FeatureApriltag::FeatureApriltag(
const std::string _type,
const Eigen::VectorXd & _measurement,
const Eigen::MatrixXd & _meas_uncertainty,
const int _tag_id,
const Vector8d & _corners_vec,
UncertaintyType _uncertainty_type) :
FeatureBase(_type, _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_(_tag_id),
tag_corners_(4)
{
setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene
for (int i = 0; i < 4; i++)
{
tag_corners_[i].x = _corners_vec[2*i];
tag_corners_[i].y = _corners_vec[2*i+1];
}
}
inline FeatureApriltag::~FeatureApriltag()
{
//
}
inline int FeatureApriltag::getTagId() const
{
return tag_id_;
}
inline const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
{
return tag_corners_;
}
} // namespace wolf
#endif
......@@ -19,68 +19,87 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef FEATURE_APRILTAG_POSE_H_
#define FEATURE_APRILTAG_POSE_H_
// Wolf apriltag
#include "apriltag/feature/feature_apriltag.h"
// Wolf core
#include <core/feature/feature_base.h>
// UMich apriltag library
#include <apriltag/apriltag.h>
#include <apriltag/common/zarray.h>
// opencv
#include <opencv2/features2d.hpp>
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureApriltagPose);
FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_uncertainty,
//class FeatureApriltagPose
class FeatureApriltagPose : public FeatureApriltag
{
public:
FeatureApriltagPose(
const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_info,
const int _tag_id,
const Vector8d & _corners_vec,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
~FeatureApriltagPose() override;
double getRepError1() const;
double getRepError2() const;
bool getUserotation() const;
private:
double rep_error1_;
double rep_error2_;
bool use_rotation_;
};
inline FeatureApriltagPose::FeatureApriltagPose(
const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_info,
const int _tag_id,
const apriltag_detection_t & _det,
const Vector8d & _corners_vec,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type) :
FeatureBase("FeatureApriltag", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_corners_(4),
detection_ (_det),
FeatureApriltag("FeatureApriltagPose", _measurement, _meas_info, _tag_id, _corners_vec, _uncertainty_type),
rep_error1_(_rep_error1),
rep_error2_(_rep_error2),
use_rotation_(_use_rotation)
{
assert(_det.id == _tag_id && "Tag ID and detection ID do not match!");
setTrackId(_tag_id);
for (int i = 0; i < 4; i++)
{
tag_corners_[i].x = detection_.p[i][0];
tag_corners_[i].y = detection_.p[i][1];
}
}
FeatureApriltag::~FeatureApriltag()
inline FeatureApriltagPose::~FeatureApriltagPose()
{
//
}
double FeatureApriltag::getTagId() const
{
return tag_id_;
}
const apriltag_detection_t& FeatureApriltag::getDetection() const
{
return detection_;
}
const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
{
return tag_corners_;
}
double FeatureApriltag::getRepError1() const
inline double FeatureApriltagPose::getRepError1() const
{
return rep_error1_;
}
double FeatureApriltag::getRepError2() const
inline double FeatureApriltagPose::getRepError2() const
{
return rep_error2_;
}
bool FeatureApriltag::getUserotation() const
inline bool FeatureApriltagPose::getUserotation() const
{
return use_rotation_;
}
} // namespace wolf
#endif
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef FEATURE_APRILTAG_PROJ_H_
#define FEATURE_APRILTAG_PROJ_H_
// Wolf apriltag
#include "apriltag/feature/feature_apriltag.h"
// Wolf core
#include <core/feature/feature_base.h>
// UMich apriltag library
#include <apriltag/apriltag.h>
#include <apriltag/common/zarray.h>
// opencv
#include <opencv2/features2d.hpp>
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureApriltagProj);
//class FeatureApriltagProj
class FeatureApriltagProj : public FeatureApriltag
{
public:
FeatureApriltagProj(
const Eigen::Vector8d & _measurement,
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,
const double _tag_width,
const Eigen::Vector7d& _pose_pnp,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureApriltagProj() override;
const Eigen::Vector7d& getPosePnp() const;
double getTagWidth() const;
private:
Eigen::Vector7d pose_pnp_;
double tag_width_;
};
inline FeatureApriltagProj::FeatureApriltagProj(
const Eigen::Vector8d & _measurement,
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,