Skip to content
Snippets Groups Projects
Commit 1254b01e authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Resolve "Subscriber&processor for landmark external detections"

parent 1f83c15f
No related branches found
No related tags found
2 merge requests!466devel->main,!462Resolve "Subscriber&processor for landmark external detections"
Showing
with 1348 additions and 159 deletions
workflow:
rules:
- if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
- if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
when: never
- if: '$CI_COMMIT_BRANCH'
stages:
- license
- build_and_test
......
......@@ -108,12 +108,13 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
# ============ HEADERS ============
SET(HDRS_CAPTURE
include/${PROJECT_NAME}/capture/capture_base.h
include/${PROJECT_NAME}/capture/capture_diff_drive.h
include/${PROJECT_NAME}/capture/capture_landmarks_external.h
include/${PROJECT_NAME}/capture/capture_motion.h
include/${PROJECT_NAME}/capture/capture_odom_2d.h
include/${PROJECT_NAME}/capture/capture_odom_3d.h
include/${PROJECT_NAME}/capture/capture_pose.h
include/${PROJECT_NAME}/capture/capture_void.h
include/${PROJECT_NAME}/capture/capture_diff_drive.h
)
SET(HDRS_COMMON
include/${PROJECT_NAME}/common/factory.h
......@@ -133,11 +134,15 @@ SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_pose_2d.h
include/${PROJECT_NAME}/factor/factor_pose_3d.h
include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_position_2d.h
include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_position_3d.h
include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
......@@ -185,6 +190,7 @@ SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/processor_pose.h
include/${PROJECT_NAME}/processor/processor_tracker.h
include/${PROJECT_NAME}/processor/processor_tracker_feature.h
include/${PROJECT_NAME}/processor/processor_tracker_feature_landmark_external.h
include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
include/${PROJECT_NAME}/processor/track_matrix.h
)
......@@ -244,12 +250,13 @@ SET(HDRS_YAML
# ============ SOURCES ============
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
src/capture/capture_diff_drive.cpp
src/capture/capture_landmarks_external.cpp
src/capture/capture_motion.cpp
src/capture/capture_odom_2d.cpp
src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
src/capture/capture_void.cpp
src/capture/capture_diff_drive.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
......@@ -294,6 +301,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_feature_landmark_external.cpp
src/processor/processor_tracker_landmark.cpp
src/processor/track_matrix.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--------
#pragma once
//Wolf includes
#include "core/capture/capture_base.h"
namespace wolf {
struct LandmarkDetection
{
int id; // id of landmark
Eigen::VectorXd measure; // either pose or position
Eigen::MatrixXd covariance; // covariance of the measure
double quality; // [0, 1] quality of the detection
};
WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
//class CaptureLandmarksExternal
class CaptureLandmarksExternal : public CaptureBase
{
protected:
std::vector<LandmarkDetection> detections_;
public:
CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const std::vector<int>& _ids = {},
const std::vector<Eigen::VectorXd>& _detections = {},
const std::vector<Eigen::MatrixXd>& _covs = {},
const std::vector<double>& _qualities = {});
~CaptureLandmarksExternal() override;
std::vector<LandmarkDetection> getDetections() const {return detections_;};
void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality);
};
} //namespace wolf
\ No newline at end of file
......@@ -142,11 +142,11 @@ class FactorRelativePose2d : public FactorAnalytic
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
......@@ -175,8 +175,8 @@ inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
......@@ -211,8 +211,8 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
......
......@@ -65,6 +65,8 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
Eigen::Vector3d residual() const;
};
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
......@@ -132,31 +134,49 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
T o_ref = _o_ref[0];
T o_target = _o_target[0];
T o_sensor = _o_sensor[0];
const T& o_ref = _o_ref[0];
const T& o_target = _o_target[0];
const T& o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
// Expected measurement
// r1_p_r1_s1 = ps
// r2_p_r2_s2 = ps
// r1_R_s1 = R(os)
// r2_R_s2 = R(os)
// w_R_r1 = R(o1)
// w_R_r2 = R(o2)
// ----------------------------------------
// s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
// s1_R_r1*r1_R_w*w_p_r1_w +
// s1_R_r1*r1_R_w*w_p_w_r2 +
// s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
// s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
expected_measurement(2) = o_target - o_ref;
// FRAME CASE
if (not getFrameOtherList().empty())
{
// Expected measurement
// r1_p_s1 = p_sensor
// r2_p_s2 = p_sensor
// r1_R_s1 = R(o_sensor)
// r2_R_s2 = R(o_sensor)
// w_p_r1 = p_ref
// w_p_r2 = p_target
// w_R_r1 = R(o_ref)
// w_R_r2 = R(o_target)
// ----------------------------------------
// s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 + w_R_r2*r2_p_s2 - w_p_r1) - r1_p_s1)
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
expected_measurement(2) = o_target - o_ref;
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
// Expected measurement
// r1_p_s1 = p_sensor
// r2_p_s2 = p_sensor
// r1_R_s1 = R(o_sensor)
// r2_R_s2 = R(o_sensor)
// w_p_r1 = p_ref
// w_p_r2 = p_target
// w_R_r1 = R(o_ref)
// w_R_r2 = R(o_target)
// ----------------------------------------
// s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
expected_measurement(2) = o_target - o_ref - o_sensor;
}
// Error
er = expected_measurement - getMeasurement().cast<T>();
......@@ -186,6 +206,34 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
return true;
}
inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
{
Eigen::Vector3d res;
Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
p_sensor = getCapture()->getSensorP()->getState();
o_sensor = getCapture()->getSensorO()->getState();
// FRAME CASE
if (not getFrameOtherList().empty())
{
p_ref = getFrameOther()->getP()->getState();
o_ref = getFrameOther()->getO()->getState();
p_target = getCapture()->getFrame()->getP()->getState();
o_target = getCapture()->getFrame()->getO()->getState();
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
p_ref = getCapture()->getFrame()->getP()->getState();
o_ref = getCapture()->getFrame()->getO()->getState();
p_target = getLandmarkOther()->getP()->getState();
o_target = getLandmarkOther()->getO()->getState();
}
operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
} // namespace wolf
#endif
......@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* factor_relative_pose_3d.h
*
* Created on: Oct 7, 2016
* Author: jsola
*/
#ifndef FACTOR_RELATIVE_POSE_3D_H_
#define FACTOR_RELATIVE_POSE_3D_H_
......@@ -48,24 +42,31 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose3d() override = default;
template<typename T>
bool operator ()(const T* const _p_current,
const T* const _q_current,
const T* const _p_past,
const T* const _q_past,
bool operator ()(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _residuals) const;
template<typename T>
bool expectation(const T* const _p_current,
const T* const _q_current,
const T* const _p_past,
const T* const _q_past,
bool expectation(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const;
Eigen::VectorXd expectation() const;
Eigen::Vector7d expectation() const;
private:
template<typename T>
......@@ -118,39 +119,64 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
_top, // topology
_ftr_current_ptr, // feature
nullptr, // frame other
nullptr, // capture other
nullptr, // feature other
_lmk_ptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_lmk_ptr->getP(), // landmark P
_lmk_ptr->getO(), // landmark Q
_ftr_current_ptr->getFrame()->getP(), // current frame P
_ftr_current_ptr->getFrame()->getO()) // current frame Q
{
MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
template<typename T>
inline bool FactorRelativePose3d::expectation(const T* const _p_current,
const T* const _q_current,
const T* const _p_past,
const T* const _q_past,
T* _expectation_dp,
T* _expectation_dq) const
inline bool FactorRelativePose3d::expectation(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
Eigen::Map<const Eigen::Matrix<T,3,1> > p_past(_p_past);
Eigen::Map<const Eigen::Quaternion<T> > q_past(_q_past);
Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_target);
Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
// std::cout << "p_current: " << p_current(0) << " "
// << p_current(1) << " "
// << p_current(2) << "\n";
// std::cout << "q_current: " << q_current.coeffs()(0) << " "
// << q_current.coeffs()(1) << " "
// << q_current.coeffs()(2) << " "
// << q_current.coeffs()(3) << "\n";
// std::cout << "p_past: " << p_past(0) << " "
// << p_past(1) << " "
// << p_past(2) << "\n";
// std::cout << "q_past: " << q_past.coeffs()(0) << " "
// << q_past.coeffs()(1) << " "
// << q_past.coeffs()(2) << " "
// << q_past.coeffs()(3) << "\n";
// std::cout << "p_target: " << p_target(0) << " "
// << p_target(1) << " "
// << p_target(2) << "\n";
// std::cout << "q_target: " << q_target.coeffs()(0) << " "
// << q_target.coeffs()(1) << " "
// << q_target.coeffs()(2) << " "
// << q_target.coeffs()(3) << "\n";
// std::cout << "p_ref: " << p_ref(0) << " "
// << p_ref(1) << " "
// << p_ref(2) << "\n";
// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
// << q_ref.coeffs()(1) << " "
// << q_ref.coeffs()(2) << " "
// << q_ref.coeffs()(3) << "\n";
// estimate motion increment, dp, dq;
expectation_dp = q_past.conjugate() * (p_current - p_past);
expectation_dq = q_past.conjugate() * q_current;
expectation_dp = q_ref.conjugate() * (p_target - p_ref);
expectation_dq = q_ref.conjugate() * q_target;
// std::cout << "exp_p: " << expectation_dp(0) << " "
// << expectation_dp(1) << " "
......@@ -163,34 +189,38 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_current,
return true;
}
inline Eigen::VectorXd FactorRelativePose3d::expectation() const
inline Eigen::Vector7d FactorRelativePose3d::expectation() const
{
Eigen::VectorXd exp(7);
Eigen::Vector7d exp;
auto frm_current = getFeature()->getFrame();
auto frm_past = getFrameOther();
const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
const Eigen::VectorXd frame_past_ori = frm_past->getO()->getState();
// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
expectation(frame_current_pos.data(),
frame_current_ori.data(),
frame_past_pos.data(),
frame_past_ori.data(),
auto lmk = getLandmarkOther();
const Eigen::VectorXd target_pos = ( frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
const Eigen::VectorXd target_ori = ( frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
const Eigen::VectorXd ref_pos = ( frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState());
const Eigen::VectorXd ref_ori = ( frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState());
// std::cout << "target_pos: " << target_pos.transpose() << std::endl;
// std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl;
// std::cout << "ref_pos: " << ref_pos.transpose() << std::endl;
// std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl;
expectation(target_pos.data(),
target_ori.data(),
ref_pos.data(),
ref_ori.data(),
exp.data(),
exp.data()+3);
return exp;
}
template<typename T>
inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
const T* const _q_current,
const T* const _p_past,
const T* const _q_past,
T* _residuals) const
inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _residuals) const
{
/* Residual expression
......@@ -232,7 +262,7 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
expectation(_p_target, _q_target, _p_ref, _q_ref, expected.data(), expected.data()+3);
// measured motion increment, dp_m, dq_m
// Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
......
......@@ -58,7 +58,7 @@ class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativeP
/** \brief Class Destructor
*/
~FactorRelativePose3dWithExtrinsics() override;
~FactorRelativePose3dWithExtrinsics() override = default;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
......@@ -116,6 +116,8 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
_feature_ptr->getCapture()->getSensorP(),
_feature_ptr->getCapture()->getSensorO())
{
assert(_feature_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_feature_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
}
inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
......@@ -143,45 +145,59 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
{
}
inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
{
//
}
template<typename T>
inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// Maps
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_target);
Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_target);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
Eigen::Map<const Eigen::Quaternion<T>> q_w_t(_o_target);
Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
// WOLF_INFO("p_sensor: ", p_r_s.transpose());
// WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
// WOLF_INFO("p_ref: ", p_w_r.transpose());
// WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose());
// WOLF_INFO("p_target: ", p_w_t.transpose());
// WOLF_INFO("o_target: ", q_w_t.coeffs().transpose());
// 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);
Eigen::Quaternion<T> expected_q;
Eigen::Matrix<T,3,1> expected_p;
// FRAME CASE
if (not getFrameOtherList().empty())
{
expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - p_w_r) - p_r_s);
expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t * q_r_s;
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t;
}
// Measurement
Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>());
Eigen::Vector3d p_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); // true error function for which the covariance should be computed
err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
// err.head(3) = q_l_c_meas * (p_meas.cast<T>() - expected_p);
err.head(3) = (p_meas - expected_p);
err.tail(3) = wolf::log_q(q_l_c_meas * expected_q); // true error function for which the covariance should be computed
//err.tail(3) = T(2)*(q_l_c_meas * expected_q).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
......@@ -192,15 +208,27 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
{
Eigen::Vector6d res;
double * p_camera, * o_camera, * p_ref, * o_ref, * p_target, * o_target;
p_camera = getCapture()->getSensorP()->getState().data();
o_camera = getCapture()->getSensorO()->getState().data();
p_ref = getCapture()->getFrame()->getP()->getState().data();
o_ref = getCapture()->getFrame()->getO()->getState().data();
p_target = getLandmarkOther()->getP()->getState().data();
o_target = getLandmarkOther()->getO()->getState().data();
operator() (p_camera, o_camera, p_ref, o_ref, p_target, o_target, res.data());
Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
p_sensor = getCapture()->getSensorP()->getState();
o_sensor = getCapture()->getSensorO()->getState();
// FRAME CASE
if (not getFrameOtherList().empty())
{
p_ref = getFrameOther()->getP()->getState();
o_ref = getFrameOther()->getO()->getState();
p_target = getCapture()->getFrame()->getP()->getState();
o_target = getCapture()->getFrame()->getO()->getState();
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
p_ref = getCapture()->getFrame()->getP()->getState();
o_ref = getCapture()->getFrame()->getO()->getState();
p_target = getLandmarkOther()->getP()->getState();
o_target = getLandmarkOther()->getO()->getState();
}
operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
......
//--------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--------
#pragma once
//Wolf includes
#include "core/factor/factor_analytic.h"
#include "core/landmark/landmark_base.h"
#include "core/math/rotations.h"
#include <Eigen/StdVector>
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorRelativePosition2d);
//class
class FactorRelativePosition2d : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePosition2d",
_top,
_ftr_ptr,
_frame_other_ptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP())
{
//
}
/** \brief Constructor of category FAC_FEATURE
**/
FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePosition2d",
_top,
_ftr_ptr,
nullptr,
nullptr,
_ftr_other_ptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_other_ptr->getFrame()->getP() )
{
//
}
/** \brief Constructor of category FAC_LANDMARK
**/
FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePosition2d",
_top,
_ftr_ptr,
nullptr,
nullptr,
nullptr,
_landmark_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_landmark_other_ptr->getP())
{
//
}
~FactorRelativePosition2d() override = default;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override
{
return 2;
}
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
**/
Eigen::VectorXd evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
/** \brief Returns the jacobians evaluated in the states provided
*
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
};
/// IMPLEMENTATION ///
inline Eigen::VectorXd FactorRelativePosition2d::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
Eigen::VectorXd residual(2);
Eigen::VectorXd expected_measurement(2);
// Expected measurement
Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
expected_measurement = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
// Residual
residual = expected_measurement - getMeasurement();
residual = getMeasurementSquareRootInformationUpper() * residual;
return residual;
}
inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 3);
assert(_st_vector.size() == 3);
assert(_compute_jacobian.size() == 3);
if (_compute_jacobian[0])
{
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
-cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
}
if (_compute_jacobian[1])
{
jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
-(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
}
if (_compute_jacobian[2])
{
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
}
}
inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 3);
assert(_st_vector.size() == 3);
assert(_compute_jacobian.size() == 3);
if (_compute_jacobian[0])
{
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
-cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
}
if (_compute_jacobian[1])
{
jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
-(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
}
if (_compute_jacobian[2])
{
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
}
}
inline void FactorRelativePosition2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 3);
jacobians[0] = (Eigen::MatrixXd(2,2) <<
-cos(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
jacobians[1] = (Eigen::MatrixXd(2,1) <<
-(getStateBlockPtrVector()[2]->getState()(0)
- getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ (getStateBlockPtrVector()[2]->getState()(1)
- getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
-(getStateBlockPtrVector()[2]->getState()(0)
- getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
- (getStateBlockPtrVector()[2]->getState()(1)
- getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0))).finished();
jacobians[2] = (Eigen::MatrixXd(2,2) <<
cos(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)),
cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
}
} // namespace wolf
\ No newline at end of file
//--------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--------
#pragma once
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorRelativePosition2dWithExtrinsics);
//class
class FactorRelativePosition2dWithExtrinsics : public FactorAutodiff<FactorRelativePosition2dWithExtrinsics, 2, 2, 1, 2, 2, 1>
{
public:
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition2dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
};
inline FactorRelativePosition2dWithExtrinsics::FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition2dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
template<typename T>
inline bool FactorRelativePosition2dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T,2,1> > res(_residuals);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
T o_ref = _o_ref[0];
T o_sensor = _o_sensor[0];
// Expected measurement
Eigen::Matrix<T, 2, 1> expected_measurement = Eigen::Rotation2D<T>(-o_sensor) *
(-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (p_target - p_ref));
// Residuals
res = getMeasurementSquareRootInformationUpper() * (expected_measurement - getMeasurement());
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
// Eigen::MatrixXs J(3,6);
// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorRelativePosition2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePosition2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePosition2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
return true;
}
} // namespace wolf
\ No newline at end of file
//--------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--------
#pragma once
#include "core/factor/factor_autodiff.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePosition3d);
//class
class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d,3,3,4,3>
{
public:
FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition3d() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _q_ref,
const T* const _p_target,
T* _residuals) const;
private:
template<typename T>
void printRes(const Eigen::Matrix<T, 3, 1>& r) const;
};
template<typename T>
inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
std::cout << r(0) << std::endl;
std::cout << r(1) << std::endl;
std::cout << r(2) << std::endl;
}
template<>
inline void FactorRelativePosition3d::printRes (const Eigen::Matrix<double,3,1> & r) const
{
std::cout << "Scalar residual = " << std::endl;
std::cout << r.transpose() << std::endl;
}
inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition3d", // type
_top, // topology
_ftr_current_ptr, // feature
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
nullptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_frame_past_ptr->getP(), // past frame P
_frame_past_ptr->getO(), // past frame Q
_ftr_current_ptr->getFrame()->getP()) // current frame P
{
MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition3d", // type
_top, // topology
_ftr_current_ptr, // feature
nullptr, // frame other
nullptr, // capture other
nullptr, // feature other
_lmk_ptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_ftr_current_ptr->getFrame()->getP(), // frame P
_ftr_current_ptr->getFrame()->getO(), // frame Q
_lmk_ptr->getP()) // landmark P
{
MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
template<typename T>
inline bool FactorRelativePosition3d::operator ()(const T* const _p_ref,
const T* const _q_ref,
const T* const _p_target,
T* _residuals) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals);
// std::cout << "p_ref: " << p_ref(0) << " "
// << p_ref(1) << " "
// << p_ref(2) << "\n";
// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
// << q_ref.coeffs()(1) << " "
// << q_ref.coeffs()(2) << " "
// << q_ref.coeffs()(3) << "\n";
// std::cout << "p_target: " << p_target(0) << " "
// << p_target(1) << " "
// << p_target(2) << "\n";
residuals = getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref)));
return true;
}
} /* namespace wolf */
\ No newline at end of file
//--------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--------
#pragma once
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorRelativePosition3dWithExtrinsics);
//class
class FactorRelativePosition3dWithExtrinsics : public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4>
{
public:
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition3dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
Eigen::Vector3d residual() const;
double cost() const;
};
inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition3dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
}
template<typename T>
inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// Maps
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
Eigen::Map<Eigen::Matrix<T,3,1>> residuals(_residuals);
// WOLF_INFO("p_sensor: ", p_r_s.transpose());
// WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
// WOLF_INFO("p_ref: ", p_w_r.transpose());
// WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose());
// WOLF_INFO("p_target: ", p_w_t.transpose());
// Expected measurement
Eigen::Matrix<T,3,1> expected_p;
expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (getMeasurement() - expected_p);
return true;
}
inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const
{
Eigen::Vector3d res;
Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target;
p_sensor = getCapture()->getSensorP()->getState();
o_sensor = getCapture()->getSensorO()->getState();
// FRAME CASE
if (not getFrameOtherList().empty())
{
p_ref = getFrameOther()->getP()->getState();
o_ref = getFrameOther()->getO()->getState();
p_target = getCapture()->getFrame()->getP()->getState();
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
p_ref = getCapture()->getFrame()->getP()->getState();
o_ref = getCapture()->getFrame()->getO()->getState();
p_target = getLandmarkOther()->getP()->getState();
}
operator() (p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
inline double FactorRelativePosition3dWithExtrinsics::cost() const
{
return residual().squaredNorm();
}
} // namespace wolf
\ No newline at end of file
......@@ -113,7 +113,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
LandmarkBasePtr getLandmark(const unsigned int& _id);
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
......
......@@ -320,6 +320,13 @@ inline void compose(const VectorComposite& _x1,
_c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
VectorComposite c("PO", {2,1});
compose(x1, x2, c);
return c;
}
inline void compose(const VectorComposite& _x1,
const VectorComposite& _x2,
VectorComposite& _c,
......
......@@ -62,6 +62,12 @@ struct PriorOptions
};
WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
struct Reference
{
StateBlockPtr p_ptr;
StateBlockPtr o_ptr;
};
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
......@@ -90,6 +96,8 @@ class Problem : public std::enable_shared_from_this<Problem>
VectorComposite transformation_;
mutable std::mutex mut_transform_;
Reference local_reference_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
......@@ -105,6 +113,10 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
SizeEigen getDim() const;
const StateStructure& getFrameStructure() const;
StateBlockPtr getLocalReferenceP();
StateBlockConstPtr getLocalReferenceP() const;
StateBlockPtr getLocalReferenceO();
StateBlockConstPtr getLocalReferenceO() const;
protected:
void appendToStructure(const StateStructure& _structure);
......@@ -453,6 +465,26 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
inline StateBlockPtr Problem::getLocalReferenceP()
{
return local_reference_.p_ptr;
}
inline StateBlockConstPtr Problem::getLocalReferenceP() const
{
return local_reference_.p_ptr;
}
inline StateBlockPtr Problem::getLocalReferenceO()
{
return local_reference_.o_ptr;
}
inline StateBlockConstPtr Problem::getLocalReferenceO() const
{
return local_reference_.o_ptr;
}
inline TreeManagerBaseConstPtr Problem::getTreeManager() const
{
return tree_manager_;
......
//--------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--------
#pragma once
#include "core/common/wolf.h"
#include "core/processor/processor_tracker_feature.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
{
double filter_quality_th; ///< min quality to consider the detection
double filter_dist_th; ///< for considering tracked detection: distance threshold to previous detection
unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
double time_span; ///< for keyframe voting: time span since last frame
ParamsProcessorTrackerFeatureLandmarkExternal() = default;
ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
const wolf::ParamsServer & _server):
ParamsProcessorTrackerFeature(_unique_name, _server)
{
filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter_quality_th");
filter_dist_th = _server.getParam<double> (prefix + _unique_name + "/filter_dist_th");
filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
}
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureLandmarkExternal);
//Class
class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
{
public:
ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tracker_feature);
~ProcessorTrackerFeatureLandmarkExternal() override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureLandmarkExternal, ParamsProcessorTrackerFeatureLandmarkExternal);
void configure(SensorBasePtr _sensor) override { };
protected:
ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
/** \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
*
* 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.
*
* \return the number of features tracked
*/
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 _last_feature input feature in last capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature) override;
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
* meaning that it wants to create a KeyFrame at the \b last Capture.
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override;
/** \brief Detect new Features
* \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.
*
* 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.
*
* 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_.
*/
unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out) override;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* This function emplaces a factor of the appropriate type for the derived processor.
*/
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr) override;
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* extract the detections and fill detections_incoming_ (FeaturePtrList)
*/
void preProcess() override;
/** Post-process
*
* This is called by process() after finishing the processing algorithm.
*
* It does nothing for now
*/
void postProcess() override {};
void advanceDerived() override;
void resetDerived() override;
double detectionDistance(FeatureBasePtr _ftr1,
FeatureBasePtr _ftr2,
const VectorComposite& _pose1,
const VectorComposite& _pose2,
const VectorComposite& _pose_sen) const;
};
inline ProcessorTrackerFeatureLandmarkExternal::ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tfle) :
ProcessorTrackerFeature("ProcessorTrackerFeatureLandmarkExternal", "PO", 0, _params_tfle),
params_tfle_(_params_tfle)
{
//
}
inline ProcessorTrackerFeatureLandmarkExternal::~ProcessorTrackerFeatureLandmarkExternal()
{
//
}
inline bool ProcessorTrackerFeatureLandmarkExternal::correctFeatureDrift(const FeatureBasePtr _origin_feature,
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature)
{
return true;
}
} // namespace wolf
\ No newline at end of file
......@@ -88,7 +88,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* - createLandmark() : creates a Landmark using a new Feature <=== IMPLEMENT
* - findLandmarks() : find the new Landmarks again in \b incoming <=== IMPLEMENT
* - establishFactors() : which calls the pure virtual:
* - createFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
* - emplaceFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
*
* Should you need extra functionality for your derived types, you can overload these two methods,
*
......
......@@ -19,16 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file utils_gtest.h
* \brief Some utils for gtest
* \author Jeremie Deray
* Created on: 26/09/2016
* Eigen macros extension by: Joan Sola on 26/04/2017
*/
#ifndef WOLF_UTILS_GTEST_H
#define WOLF_UTILS_GTEST_H
#pragma once
#include <gtest/gtest.h>
......@@ -144,24 +135,58 @@ TEST(Test, Foo)
}, \
C_expect, C_actual);
#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
}, \
Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
}, \
Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
MatrixXd er = lhs - rhs; \
#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
}, \
C_expect, C_actual);
#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
}, \
C_expect, C_actual);
#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
assert(lhs.size() == 3 and rhs.size() == 3); \
Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
MatrixXd er = lhs - rhs; \
#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
assert(lhs.size() == 3 and rhs.size() == 3); \
Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
} // namespace testing
#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
assert(lhs.size() == 7 and rhs.size() == 7); \
Eigen::Vector4d er; \
er.head(3) = lhs.head(3) - rhs.head(3); \
er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
assert(lhs.size() == 7 and rhs.size() == 7); \
Eigen::Vector4d er; \
er.head(3) = lhs.head(3) - rhs.head(3); \
er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
#endif /* WOLF_UTILS_GTEST_H */
} // namespace testing
\ No newline at end of file
//--------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--------
#include "core/capture/capture_landmarks_external.h"
namespace wolf{
CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const std::vector<int>& _ids,
const std::vector<Eigen::VectorXd>& _detections,
const std::vector<Eigen::MatrixXd>& _covs,
const std::vector<double>& _qualities) :
CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
{
if (_ids.size() != _detections.size() or
_ids.size() != _covs.size() or
_ids.size() != _qualities.size())
throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
for (auto i = 0; i < _detections.size(); i++)
addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
}
CaptureLandmarksExternal::~CaptureLandmarksExternal()
{
//
}
void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
{
detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
}
} // namespace wolf
......@@ -65,21 +65,46 @@ void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
landmark_list_.remove(_landmark_ptr);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
LandmarkBaseConstPtr MapBase::getLandmark(const unsigned int& _id) const
{
YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
auto lmk_it = std::find_if(landmark_list_.begin(),
landmark_list_.end(),
[&](LandmarkBaseConstPtr lmk)
{
return lmk->id() == _id;
}); // lambda function for the find_if
if (lmk_it == landmark_list_.end())
return nullptr;
return (*lmk_it);
}
LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
{
auto lmk_it = std::find_if(landmark_list_.begin(),
landmark_list_.end(),
[&](LandmarkBasePtr lmk)
{
return lmk->id() == _id;
}); // lambda function for the find_if
unsigned int nlandmarks = map["nlandmarks"].as<unsigned int>();
if (lmk_it == landmark_list_.end())
return nullptr;
assert(nlandmarks == map["landmarks"].size() && "Number of landmarks in map file does not match!");
return (*lmk_it);
}
for (unsigned int i = 0; i < nlandmarks; i++)
void MapBase::load(const std::string& _map_file_dot_yaml)
{
YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
for (unsigned int i = 0; i < map["landmarks"].size(); i++)
{
YAML::Node lmk_node = map["landmarks"][i];
LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
lmk_ptr->link(shared_from_this());
}
}
void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
......@@ -87,10 +112,8 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
YAML::Emitter emitter;
emitter << YAML::BeginMap;
emitter << "map name" << _map_name;
emitter << "date-time" << dateTimeNow(); // Get date and time for archiving purposes
emitter << "nlandmarks" << getLandmarkList().size();
emitter << "map_name" << _map_name;
emitter << "date_time" << dateTimeNow(); // Get date and time for archiving purposes
emitter << "landmarks" << YAML::BeginSeq;
......
......@@ -200,11 +200,11 @@ void ProcessorTrackerFeature::establishFactors()
auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()");
assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) && "not emplaced factor returned by emplaceFactor()");
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
" origin: " , feature_in_origin->id() ,
" from last: " , feature_in_last->id() );
WOLF_DEBUG_COND(fac_ptr, "New factor: track: " , feature_in_last->trackId(),
" origin: " , feature_in_origin->id() ,
" from last: " , feature_in_last->id() );
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment