diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 40101fc3cdbbee48bf214edd27cf4f3143409528..cdc737865745bdbeb7158e9c1032cbe0c559d5d2 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -114,8 +114,6 @@ license_headers: - key: wolf-focal paths: - ci_deps/wolf/ - except: - - master before_script: - *preliminaries_definition - *install_wolf_definition @@ -133,8 +131,6 @@ build_and_test:bionic: - key: visionutils-bionic paths: - ci_deps/vision_utils/ - except: - - master before_script: - *preliminaries_definition - *install_wolf_definition @@ -154,8 +150,6 @@ build_and_test:focal: - key: visionutils-focal paths: - ci_deps/vision_utils/ - except: - - master before_script: - *preliminaries_definition - *install_wolf_definition diff --git a/cmake_modules/wolfvisionConfig.cmake b/cmake_modules/wolfvisionConfig.cmake index aaa8e63988de29e8adde091e35304030a3b1205f..b5cb73640a527c9d74f70d07ac8e82a03ea4d45c 100644 --- a/cmake_modules/wolfvisionConfig.cmake +++ b/cmake_modules/wolfvisionConfig.cmake @@ -48,7 +48,11 @@ macro(wolf_report_not_found REASON_MSG) # FindPackage() use the camelcase library name, not uppercase. if (wolfvision_FIND_QUIETLY) message(STATUS "Failed to find wolfvision- " ${REASON_MSG} ${ARGN}) +<<<<<<< HEAD elseif(wolfvision_FIND_REQUIRED) +======= + elseif (wolfvision_FIND_REQUIRED) +>>>>>>> devel message(FATAL_ERROR "Failed to find wolfvision - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp index d7b3affbaabd67b3484ce89b492213916755a964..5a13abcfb91f6bdac694aabdbba3f1f0b8fa71a7 100644 --- a/demos/demo_processor_bundle_adjustment.cpp +++ b/demos/demo_processor_bundle_adjustment.cpp @@ -173,9 +173,9 @@ int main(int argc, char** argv) camera->process(image); // solve only when new KFs are added - if (problem->getTrajectory()->getFrameMap().size() > number_of_KFs) + if (problem->getTrajectory()->size() > number_of_KFs) { - number_of_KFs = problem->getTrajectory()->getFrameMap().size(); + number_of_KFs = problem->getTrajectory()->size(); std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF); std::cout << report << std::endl; if (number_of_KFs > 5) diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h index fa938812d54188675d06eeec09cef3d05850f4de..c31f03fb61799b4e3fdf98d0fa787f1c59e2dda2 100644 --- a/include/vision/capture/capture_image.h +++ b/include/vision/capture/capture_image.h @@ -124,7 +124,7 @@ class CaptureImage : public CaptureBase const TracksMap& getTracksOrigin() const {return tracks_origin_;} void setTracksOrigin(const TracksMap& _tracks){tracks_origin_ = _tracks;} - bool getLastWasRepopulated(){return last_was_repopulated_;} + bool getLastWasRepopulated() const {return last_was_repopulated_;} void setLastWasRepopulated(bool _repopulated){last_was_repopulated_ = _repopulated;} void addKeyPoint(const WKeyPoint& _wkp); diff --git a/include/vision/factor/factor_ahp.h b/include/vision/factor/factor_ahp.h index 4c47c0912e357b79cc2728c1cedb4555467e227b..833a3f3c34fb58856af214444f78a172d51b3225 100644 --- a/include/vision/factor/factor_ahp.h +++ b/include/vision/factor/factor_ahp.h @@ -103,9 +103,9 @@ inline FactorAhp::FactorAhp(const FeatureBasePtr& _ftr_ptr, inline Eigen::VectorXd FactorAhp::expectation() const { - FrameBasePtr frm_current = getFeature()->getCapture()->getFrame(); - FrameBasePtr frm_anchor = getFrameOther(); - LandmarkBasePtr lmk = getLandmarkOther(); + auto frm_current = getFeature()->getCapture()->getFrame(); + auto frm_anchor = getFrameOther(); + auto lmk = getLandmarkOther(); const Eigen::MatrixXd frame_current_pos = frm_current->getP()->getState(); const Eigen::MatrixXd frame_current_ori = frm_current->getO()->getState(); @@ -151,7 +151,7 @@ inline void FactorAhp::expectation(const T* const _current_frame_p, TransformType T_R0_C0 = t_r0_c0 * q_r0_c0; // current robot to current camera transform - CaptureBasePtr current_capture = this->getFeature()->getCapture(); + auto current_capture = this->getFeature()->getCapture(); Translation<T, 3> t_r1_c1 (current_capture->getSensorP()->getState().cast<T>()); Quaterniond q_r1_c1_s(Eigen::Vector4d(current_capture->getSensorO()->getState())); Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>(); diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h index 0edfcb65ef32d02ccf74b0e7ccd46ac0c233ea00..fc92af152cc27febafca51d42c16c36fd58aad0a 100644 --- a/include/vision/factor/factor_pixel_hp.h +++ b/include/vision/factor/factor_pixel_hp.h @@ -99,9 +99,9 @@ inline FactorPixelHp::FactorPixelHp(const FeatureBasePtr& _ftr_ptr, inline Eigen::VectorXd FactorPixelHp::expectation() const { - FrameBasePtr frm = getFeature()->getCapture()->getFrame(); - SensorBasePtr sen = getFeature()->getCapture()->getSensor(); - LandmarkBasePtr lmk = getLandmarkOther(); + auto frm = getFeature()->getCapture()->getFrame(); + auto sen = getFeature()->getCapture()->getSensor(); + auto lmk = getLandmarkOther(); const Eigen::MatrixXd frame_pos = frm->getP()->getState(); const Eigen::MatrixXd frame_ori = frm->getO()->getState(); diff --git a/include/vision/factor/factor_trifocal.h b/include/vision/factor/factor_trifocal.h new file mode 100644 index 0000000000000000000000000000000000000000..b2cf8514edcfef30f093d8f3c29d3f8a50114076 --- /dev/null +++ b/include/vision/factor/factor_trifocal.h @@ -0,0 +1,457 @@ +//--------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_TRIFOCAL_H_ +#define _FACTOR_TRIFOCAL_H_ + +//Wolf includes +//#include "core/common/wolf.h" +#include "core/factor/factor_autodiff.h" +#include "vision/sensor/sensor_camera.h" + +#include <vision_utils/common_class/trifocaltensor.h> +#include <vision_utils/vision_utils.h> + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(FactorTrifocal); + +using namespace Eigen; + +class FactorTrifocal : public FactorAutodiff<FactorTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4> +{ + public: + + /** \brief Class constructor + */ + FactorTrifocal(const FeatureBasePtr& _feature_1_ptr, + const FeatureBasePtr& _feature_2_ptr, + const FeatureBasePtr& _feature_own_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status); + + /** \brief Class Destructor + */ + ~FactorTrifocal() override; + + FeatureBaseConstPtr getFeaturePrev() const; + FeatureBasePtr getFeaturePrev(); + + const Vector3d& getPixelCanonical3() const + { + return pixel_canonical_3_; + } + + void setPixelCanonical3(const Vector3d& pixelCanonical3) + { + pixel_canonical_3_ = pixelCanonical3; + } + + const Vector3d& getPixelCanonical2() const + { + return pixel_canonical_2_; + } + + void setPixelCanonical2(const Vector3d& pixelCanonical2) + { + pixel_canonical_2_ = pixelCanonical2; + } + + const Vector3d& getPixelCanonical1() const + { + return pixel_canonical_1_; + } + + void setPixelCanonical1(const Vector3d& pixelCanonical1) + { + pixel_canonical_1_ = pixelCanonical1; + } + + const Matrix3d& getSqrtInformationUpper() const + { + return sqrt_information_upper; + } + + /** brief : compute the residual from the state blocks being iterated by the solver. + **/ + template<typename T> + bool operator ()( const T* const _pos1, + const T* const _quat1, + const T* const _pos2, + const T* const _quat2, + const T* const _pos3, + const T* const _quat3, + const T* const _sen_pos, + const T* const _sen_quat, + T* _residuals) const; + + public: + template<typename D1, typename D2, class T, typename D3> + void expectation(const MatrixBase<D1>& _wtr1, + const QuaternionBase<D2>& _wqr1, + const MatrixBase<D1>& _wtr2, + const QuaternionBase<D2>& _wqr2, + const MatrixBase<D1>& _wtr3, + const QuaternionBase<D2>& _wqr3, + const MatrixBase<D1>& _rtc, + const QuaternionBase<D2>& _rqc, + vision_utils::TrifocalTensorBase<T>& _tensor, + MatrixBase<D3>& _c2Ec1) const; + + template<typename T, typename D1> + Matrix<T, 3, 1> residual(const vision_utils::TrifocalTensorBase<T>& _tensor, + const MatrixBase<D1>& _c2Ec1) const; + + // Helper functions to be used by the above + template<class T, typename D1, typename D2, typename D3, typename D4> + Matrix<T, 3, 1> error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor, + const MatrixBase<D1>& _c2Ec1, + MatrixBase<D2>& _J_e_m1, + MatrixBase<D3>& _J_e_m2, + MatrixBase<D4>& _J_e_m3); + + private: + FeatureBaseWPtr feature_prev_ptr_; // To look for measurements + SensorCameraPtr camera_ptr_; // To look for intrinsics + Vector3d pixel_canonical_1_, pixel_canonical_2_, pixel_canonical_3_; + + Matrix3d sqrt_information_upper; + + //Print function specialized for doubles (avoid jets) + template <class T, int ROWS, int COLS> + void print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat) const; + + template <int ROWS, int COLS> + void print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat) const; + + template<class T> + void print_scalar(const T& _val) const; + + void print_scalar(const double& _val) const; +}; + +} // namespace wolf + +// Includes for implentation +#include "core/math/rotations.h" + +namespace wolf +{ + +using namespace Eigen; + +// Constructor +FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr, + const FeatureBasePtr& _feature_2_ptr, + const FeatureBasePtr& _feature_own_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff( "TRIFOCAL PLP", + TOP_GEOM, + _feature_own_ptr, + FrameBasePtrList(), + CaptureBasePtrList(), + FeatureBasePtrList({_feature_2_ptr, _feature_1_ptr}), //< this sets feature 2 (the one between the oldest and the newest) + LandmarkBasePtrList(), + _processor_ptr, + _apply_loss_function, + _status, + _feature_1_ptr->getFrame()->getP(), + _feature_1_ptr->getFrame()->getO(), + _feature_2_ptr->getFrame()->getP(), + _feature_2_ptr->getFrame()->getO(), + _feature_own_ptr->getFrame()->getP(), + _feature_own_ptr->getFrame()->getO(), + _feature_own_ptr->getCapture()->getSensorP(), + _feature_own_ptr->getCapture()->getSensorO() ), + feature_prev_ptr_(_feature_1_ptr), + camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())), + sqrt_information_upper(Matrix3d::Zero()) +{ + // First add feature_1_ptr to the list of features (because the constructor FeatureAutodiff did not do so) + //if (_feature_1_ptr) feature_other_list_.push_back(_feature_1_ptr); + + // Store some geometry elements + Matrix3d K_inv = camera_ptr_->getIntrinsicMatrix().inverse(); + pixel_canonical_1_ = K_inv * Vector3d(_feature_1_ptr->getMeasurement(0), _feature_1_ptr->getMeasurement(1), 1.0); + pixel_canonical_2_ = K_inv * Vector3d(_feature_2_ptr->getMeasurement(0), _feature_2_ptr->getMeasurement(1), 1.0); + pixel_canonical_3_ = K_inv * Vector3d(_feature_own_ptr->getMeasurement(0), _feature_own_ptr->getMeasurement(1), 1.0); + Matrix<double,3,2> J_m_u = K_inv.block(0,0,3,2); + + // extract relevant states + Vector3d wtr1 = _feature_1_ptr ->getFrame() ->getP() ->getState(); + Quaterniond wqr1 = Quaterniond(_feature_1_ptr ->getFrame() ->getO() ->getState().data() ); + Vector3d wtr2 = _feature_2_ptr ->getFrame() ->getP() ->getState(); + Quaterniond wqr2 = Quaterniond(_feature_2_ptr ->getFrame() ->getO() ->getState().data() ); + Vector3d wtr3 = _feature_own_ptr->getFrame() ->getP() ->getState(); + Quaterniond wqr3 = Quaterniond(_feature_own_ptr->getFrame() ->getO() ->getState().data() ); + Vector3d rtc = _feature_own_ptr->getCapture()->getSensorP()->getState(); + Quaterniond rqc = Quaterniond(_feature_own_ptr->getCapture()->getSensorO()->getState().data() ); + + // expectation // canonical units + vision_utils::TrifocalTensorBase<double> tensor; + Matrix3d c2Ec1; + expectation(wtr1, wqr1, + wtr2, wqr2, + wtr3, wqr3, + rtc, rqc, + tensor, c2Ec1); + + // error Jacobians // canonical units + Matrix<double,3,3> J_e_m1, J_e_m2, J_e_m3; + error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3); + + // chain rule to go from homogeneous pixel to Euclidean pixel + Matrix<double,3,2> J_e_u1 = J_e_m1 * J_m_u; + Matrix<double,3,2> J_e_u2 = J_e_m2 * J_m_u; + Matrix<double,3,2> J_e_u3 = J_e_m3 * J_m_u; + + // Error covariances induced by each of the measurement covariance // canonical units + Matrix3d Q1 = J_e_u1 * _feature_1_ptr ->getMeasurementCovariance() * J_e_u1.transpose(); + Matrix3d Q2 = J_e_u2 * _feature_2_ptr ->getMeasurementCovariance() * J_e_u2.transpose(); + Matrix3d Q3 = J_e_u3 * _feature_own_ptr ->getMeasurementCovariance() * J_e_u3.transpose(); + + // Total error covariance // canonical units + Matrix3d Q = Q1 + Q2 + Q3; + + // Sqrt of information matrix // canonical units + Eigen::LLT<Eigen::MatrixXd> llt_of_info(Q.inverse()); // Cholesky decomposition + sqrt_information_upper = llt_of_info.matrixU(); + + // Re-write info matrix (for debug only) + // double pix_noise = 1.0; + // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3d::Identity(); // one PLP (2d) and one epipolar (1d) factor +} + +// Destructor +FactorTrifocal::~FactorTrifocal() +{ +} + +inline FeatureBasePtr FactorTrifocal::getFeaturePrev() +{ + return feature_prev_ptr_.lock(); +} + +template<typename T> +bool FactorTrifocal::operator ()( const T* const _pos1, + const T* const _quat1, + const T* const _pos2, + const T* const _quat2, + const T* const _pos3, + const T* const _quat3, + const T* const _sen_pos, + const T* const _sen_quat, + T* _residuals) const +{ + + // MAPS + Map<const Matrix<T,3,1> > wtr1(_pos1); + Map<const Quaternion<T> > wqr1(_quat1); + Map<const Matrix<T,3,1> > wtr2(_pos2); + Map<const Quaternion<T> > wqr2(_quat2); + Map<const Matrix<T,3,1> > wtr3(_pos3); + Map<const Quaternion<T> > wqr3(_quat3); + Map<const Matrix<T,3,1> > rtc (_sen_pos); + Map<const Quaternion<T> > rqc (_sen_quat); + Map<Matrix<T,3,1> > res (_residuals); + + vision_utils::TrifocalTensorBase<T> tensor; + Matrix<T, 3, 3> c2Ec1; + expectation(wtr1, wqr1, wtr2, wqr2, wtr3, wqr3, rtc, rqc, tensor, c2Ec1); + res = residual(tensor, c2Ec1); + return true; +} + +template<typename D1, typename D2, class T, typename D3> +inline void FactorTrifocal::expectation(const MatrixBase<D1>& _wtr1, + const QuaternionBase<D2>& _wqr1, + const MatrixBase<D1>& _wtr2, + const QuaternionBase<D2>& _wqr2, + const MatrixBase<D1>& _wtr3, + const QuaternionBase<D2>& _wqr3, + const MatrixBase<D1>& _rtc, + const QuaternionBase<D2>& _rqc, + vision_utils::TrifocalTensorBase<T>& _tensor, + MatrixBase<D3>& _c2Ec1) const +{ + + typedef Translation<T, 3> TranslationType; + typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType; + + // All input Transforms + TransformType wHr1 = TranslationType(_wtr1) * _wqr1; + TransformType wHr2 = TranslationType(_wtr2) * _wqr2; + TransformType wHr3 = TranslationType(_wtr3) * _wqr3; + TransformType rHc = TranslationType(_rtc) * _rqc ; + + // Relative camera transforms + TransformType c1Hc2 = rHc.inverse() * wHr1.inverse() * wHr2 * rHc; + TransformType c1Hc3 = rHc.inverse() * wHr1.inverse() * wHr3 * rHc; + + // Projection matrices + Matrix<T,3,4> c2Pc1 = c1Hc2.inverse().affine(); + Matrix<T,3,4> c3Pc1 = c1Hc3.inverse().affine(); + + // Trifocal tensor + _tensor.computeTensorFromProjectionMat(c2Pc1, c3Pc1); + + /* Essential matrix convention disambiguation + * + * C1 is the origin frame or reference + * C2 is another cam + * C2 is specified by R and T wrt C1 so that + * T is the position of C2 wrt C1 + * R is the orientation of C2 wrt C1 + * There is a 3d point P, noted P1 when expressed in C1 and P2 when expressed in C2: + * P1 = T + R * P2 + * + * Coplanarity condition: a' * (b x c) = 0 with {a,b,c} three coplanar vectors. + * + * The three vectors are: + * + * baseline: b = T + * ray 1 : r1 = P1 + * ray 2 : r2 = P1 - T = R*P2; + * + * so, + * + * (r1)' * (b x r2) = 0 , which develops as: + * + * P1' * (T x (R*P2)) = 0 + * P1' * [T]x * R * P2 = 0 + * P1' * c1Ec2 * P2 = 0 <--- Epipolar factor + * + * therefore: + * c1Ec2 = [T]x * R <--- Essential matrix + * + * or, if we prefer the factor P2' * c2Ec1 * P1 = 0, + * c2Ec1 = c1Ec2' = R' * [T]x (we obviate the sign change) + */ + Matrix<T,3,3> Rtr = c1Hc2.matrix().block(0,0,3,3).transpose(); + Matrix<T,3,1> t = c1Hc2.matrix().block(0,3,3,1); + _c2Ec1 = Rtr * skew(t); +// _c2Ec1 = c1Hc2.rotation().transpose() * skew(c1Hc2.translation()) ; +} + +template<typename T, typename D1> +inline Matrix<T, 3, 1> FactorTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor, + const MatrixBase<D1>& _c2Ec1) const +{ + // 1. COMMON COMPUTATIONS + + // m1, m2, m3: canonical pixels in cams 1,2,3 -- canonical means m = K.inv * u, with _u_ a homogeneous pixel [ux; uy; 1]. + Matrix<T,3,1> m1(pixel_canonical_1_.cast<T>()); + Matrix<T,3,1> m2(pixel_canonical_2_.cast<T>()); + Matrix<T,3,1> m3(pixel_canonical_3_.cast<T>()); + + // 2. TRILINEARITY PLP + + Matrix<T,2,1> e1; + vision_utils::errorTrifocalPLP(m1, m2, m3, _tensor, _c2Ec1, e1); + + // 3. EPIPOLAR + T e2; + vision_utils::errorEpipolar(m1, m2, _c2Ec1, e2); + + // 4. RESIDUAL + + Matrix<T,3,1> errors, residual; + errors << e1, e2; + residual = sqrt_information_upper.cast<T>() * errors; + + return residual; +} + +// Helper functions to be used by the above +template<class T, typename D1, typename D2, typename D3, typename D4> +inline Matrix<T, 3, 1> FactorTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor, + const MatrixBase<D1>& _c2Ec1, + MatrixBase<D2>& _J_e_m1, + MatrixBase<D3>& _J_e_m2, + MatrixBase<D4>& _J_e_m3) +{ + // 1. COMMON COMPUTATIONS + + // m1, m2, m3: canonical pixels in cams 1,2,3 -- canonical means m = K.inv * u, with _u_ a homogeneous pixel [ux; uy; 1]. + Matrix<T,3,1> m1(pixel_canonical_1_.cast<T>()); + Matrix<T,3,1> m2(pixel_canonical_2_.cast<T>()); + Matrix<T,3,1> m3(pixel_canonical_3_.cast<T>()); + + // 2. TRILINEARITY PLP + + Matrix<T,2,3> J_e1_m1, J_e1_m2, J_e1_m3; + Matrix<T,2,1> e1; + + vision_utils::errorTrifocalPLP(m1, m2, m3, _tensor, _c2Ec1, e1, J_e1_m1, J_e1_m2, J_e1_m3); + + // 3. EPIPOLAR + + T e2; + Matrix<T,1,3> J_e2_m1, J_e2_m2, J_e2_m3; + + vision_utils::errorEpipolar(m1, m2, _c2Ec1, e2, J_e2_m1, J_e2_m2); + + J_e2_m3.setZero(); // Not involved in epipolar c1->c2 + + // Compact Jacobians + _J_e_m1.topRows(2) = J_e1_m1; + _J_e_m1.row(2) = J_e2_m1; + _J_e_m2.topRows(2) = J_e1_m2; + _J_e_m2.row(2) = J_e2_m2; + _J_e_m3.topRows(2) = J_e1_m3; + _J_e_m3.row(2) = J_e2_m3; + + // 4. ERRORS + + Matrix<T,3,1> errors; + errors << e1, e2; + + return errors; + +} + +// Print function +template<class T, int ROWS, int COLS> +void FactorTrifocal::print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat) const +{} + +template<int ROWS, int COLS> +void FactorTrifocal::print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat) const +{ + std::cout << _mat << std::endl; +} + +template<class T> +void FactorTrifocal::print_scalar(const T& _val) const +{} + +void FactorTrifocal::print_scalar(const double& _val) const +{ + std::cout << _val << std::endl; +} + +} // namespace wolf + +#endif /* _FACTOR_AUTODIFF_TRIFOCAL_H_ */ diff --git a/include/vision/landmark/landmark_ahp.h b/include/vision/landmark/landmark_ahp.h index 7bb96aa69fd73ddc83ae5bf92c55f834dcbd1526..a60dc0cb890456d1c00f9d7d94f45205aaa986ee 100644 --- a/include/vision/landmark/landmark_ahp.h +++ b/include/vision/landmark/landmark_ahp.h @@ -52,8 +52,10 @@ class LandmarkAhp : public LandmarkBase const cv::Mat& getCvDescriptor() const; void setCvDescriptor(const cv::Mat& _descriptor); - const FrameBasePtr getAnchorFrame () const; - const SensorBasePtr getAnchorSensor() const; + FrameBaseConstPtr getAnchorFrame () const; + FrameBasePtr getAnchorFrame (); + SensorBaseConstPtr getAnchorSensor() const; + SensorBasePtr getAnchorSensor(); void setAnchorFrame (FrameBasePtr _anchor_frame ); void setAnchorSensor (SensorBasePtr _anchor_sensor); @@ -80,7 +82,12 @@ inline void LandmarkAhp::setCvDescriptor(const cv::Mat& _descriptor) cv_descriptor_ = _descriptor; } -inline const FrameBasePtr LandmarkAhp::getAnchorFrame() const +inline FrameBaseConstPtr LandmarkAhp::getAnchorFrame() const +{ + return anchor_frame_; +} + +inline FrameBasePtr LandmarkAhp::getAnchorFrame() { return anchor_frame_; } @@ -90,7 +97,12 @@ inline void LandmarkAhp::setAnchorFrame(FrameBasePtr _anchor_frame) anchor_frame_ = _anchor_frame; } -inline const SensorBasePtr LandmarkAhp::getAnchorSensor() const +inline SensorBaseConstPtr LandmarkAhp::getAnchorSensor() const +{ + return anchor_sensor_; +} + +inline SensorBasePtr LandmarkAhp::getAnchorSensor() { return anchor_sensor_; }