Commit a65ad541 authored by mederic_fourmy's avatar mederic_fourmy
Browse files

Fix gtests

parent dbd639db
......@@ -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})
......@@ -104,7 +104,6 @@ INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_apriltag.h
include/${PROJECT_NAME}/factor/factor_apriltag_proj.h
)
SET(HDRS_FEATURE
......
//--------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 _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"
#include "apriltag/landmark/landmark_apriltag.h"
#include "apriltag/feature/feature_apriltag.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorApriltag);
class FactorApriltag : public FactorAutodiff<FactorApriltag, 6, 3, 4, 3, 4, 3, 4>
{
public:
/** \brief Class constructor
*/
FactorApriltag(
const SensorBasePtr& _sensor_ptr,
const FrameBasePtr& _frame_ptr,
const LandmarkApriltagPtr& _landmark_other_ptr,
const FeatureApriltagPtr& _feature_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status);
/** \brief Class Destructor
*/
~FactorApriltag() override;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
template<typename T>
bool 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;
Eigen::Vector6d residual() const;
double cost() const;
// print function only for double (not Jet)
template<typename T, int Rows, int Cols>
void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const
{
// jet prints nothing
}
template<int Rows, int Cols>
void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const
{
// double prints stuff
WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
}
};
} // namespace wolf
// Include here all headers for this class
//#include <YOUR_HEADERS.h>
namespace wolf
{
FactorApriltag::FactorApriltag(
const SensorBasePtr& _sensor_ptr,
const FrameBasePtr& _frame_ptr,
const LandmarkApriltagPtr& _landmark_other_ptr,
const FeatureApriltagPtr& _feature_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorApriltag",
TOP_LMK,
_feature_ptr,
nullptr,
nullptr,
nullptr,
_landmark_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_sensor_ptr->getP(), _sensor_ptr->getO(),
_frame_ptr->getP(), _frame_ptr->getO(),
_landmark_other_ptr->getP(), _landmark_other_ptr->getO()
)
{
}
/** \brief Class Destructor
*/
FactorApriltag::~FactorApriltag()
{
//
}
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
{
// Maps
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_camera);
Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_camera);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_keyframe);
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);
// 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>();
// 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;
return true;
}
Eigen::Vector6d FactorApriltag::residual() const
{
Eigen::Vector6d 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();
p_frame = getCapture()->getFrame()->getP()->getState().data();
o_frame = getCapture()->getFrame()->getO()->getState().data();
p_tag = getLandmarkOther()->getP()->getState().data();
o_tag = getLandmarkOther()->getO()->getState().data();
operator() (p_camera, o_camera, p_frame, o_frame, p_tag, o_tag, res.data());
return res;
}
double FactorApriltag::cost() const
{
return residual().squaredNorm();
}
} // namespace wolf
#endif /* _CONSTRAINT_AUTODIFF_APRILTAG_H_ */
......@@ -87,6 +87,13 @@ class FactorApriltagProj : public FactorAutodiff<FactorApriltagProj, 8, 3, 4, 3,
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
......@@ -120,8 +127,20 @@ FactorApriltagProj::FactorApriltagProj(
_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
......@@ -174,37 +193,11 @@ bool FactorApriltagProj::operator ()(const T* const _p_camera,
//////////////////////////////////////
// Expected corner projections
//////////////////////////////////////
// Camera matrix (put somewhere else)
// get camera intrinsic parameters
Eigen::Vector4d k = getSensor()->getIntrinsic()->getState(); //[cx cy fx fy]
Eigen::Matrix3d K;
K << k(2), 0, k(0),
0, k(3), k(1),
0, 0, 1;
//////////////////////////////////////
auto feata = std::dynamic_pointer_cast<FeatureApriltagProj>(getFeature());
double tag_width = feata->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
// >>>>>>> JV: COULD THIS BE PRECOMPUTED AT CONSTRUCTION TIME?
double s = tag_width/2;
Eigen::Vector3d l_corn1; l_corn1 << -s, s, 0; // bottom left
Eigen::Vector3d l_corn2; l_corn2 << s, s, 0; // bottom right
Eigen::Vector3d l_corn3; l_corn3 << s, -s, 0; // top right
Eigen::Vector3d l_corn4; l_corn4 << -s, -s, 0; // top left
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);
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_);
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (corners_exp - getMeasurement().cast<T>());
......
......@@ -42,13 +42,13 @@ WOLF_PTR_TYPEDEFS(FeatureApriltagProj);
class FeatureApriltagProj : public FeatureBase
{
public:
FeatureApriltagProj(const Eigen::Vector8d & _measurement,
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,
const double _tag_width,
const apriltag_detection_t & _det,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,
const double _tag_width,
const apriltag_detection_t & _det,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureApriltagProj() override;
/** \brief Returns tag id
......@@ -63,7 +63,6 @@ class FeatureApriltagProj : public FeatureBase
const std::vector<cv::Point2d>& getTagCorners() const;
private:
int tag_id_;
double tag_width_;
......
......@@ -10,6 +10,8 @@ add_subdirectory(gtest)
wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag.cpp)
wolf_add_gtest(gtest_feature_apriltag_proj gtest_feature_apriltag_proj.cpp)
wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp)
wolf_add_gtest(gtest_processor_tracker_landmark_apriltag gtest_processor_tracker_landmark_apriltag.cpp)
......
//--------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--------
/**
* \file gtest_feature_apriltag_proj.cpp
*
* Created on: May 30, 2022
* \author: mfourmy
*/
#include <core/utils/utils_gtest.h>
#include "apriltag/feature/feature_apriltag_proj.h"
using namespace wolf;
class FeatureApriltag_test : public testing::Test
{
public:
Eigen::Vector8d meas;
Eigen::Matrix8d cov;
int tag_id_;
double tag_width_;
apriltag_detection_t det_;
void SetUp() override
{
meas << 1,2,3,4,5,6,7,8;
cov.setIdentity() * 2.0;
tag_id_ = 1;
tag_width_ = 0.2;
det_.id = 1;
det_.p[0][0] = 1.0;
det_.p[0][1] = -1.0;
det_.p[1][0] = 1.0;
det_.p[1][1] = 1.0;
det_.p[2][0] = -1.0;
det_.p[2][1] = 1.0;
det_.p[3][0] = -1.0;
det_.p[3][1] = -1.0;
}
};
TEST_F(FeatureApriltag_test, type)
{
auto f = std::make_shared<FeatureApriltagProj>(meas, cov, tag_id_, tag_width_, det_);
ASSERT_EQ(f->getType(), "FeatureApriltagProj");
}
TEST_F(FeatureApriltag_test, getters)
{
auto f = std::make_shared<FeatureApriltagProj>(meas, cov, tag_id_, tag_width_, det_);
ASSERT_EQ(f->getTagId(), tag_id_);
ASSERT_EQ(f->getTagWidth(), tag_width_);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment