diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9210f755f24b693a11b83550cdfabcd58c251fa8..cd81990329ddee7e5b82b04ce5cab1a28b7a3b29 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -232,7 +232,6 @@ SET(HDRS_COMMON
)
SET(HDRS_MATH
include/core/math/SE3.h
- include/core/math/pinhole_tools.h
include/core/math/rotations.h
)
SET(HDRS_UTILS
@@ -264,7 +263,6 @@ SET(HDRS_STATE_BLOCK
include/core/state_block/local_parametrization_angle.h
include/core/state_block/local_parametrization_base.h
include/core/state_block/local_parametrization_homogeneous.h
- include/core/state_block/local_parametrization_polyline_extreme.h
include/core/state_block/local_parametrization_quaternion.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
@@ -290,8 +288,6 @@ SET(HDRS_FACTOR
include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h
include/core/factor/factor_diff_drive.h
- include/core/factor/factor_epipolar.h
- include/core/factor/factor_fix_bias.h
include/core/factor/factor_odom_2D.h
include/core/factor/factor_odom_2D_analytic.h
include/core/factor/factor_odom_3D.h
@@ -375,7 +371,6 @@ SET(SRCS_FRAME
SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp
- src/state_block/local_parametrization_polyline_extreme.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
)
diff --git a/include/core/factor/factor_epipolar.h b/include/core/factor/factor_feature_dummy.h
similarity index 80%
rename from include/core/factor/factor_epipolar.h
rename to include/core/factor/factor_feature_dummy.h
index 072164bbe39ede408c97db569e73a10aaefde3a2..52831f246cf132bd1c536004e4757baec35b48a8 100644
--- a/include/core/factor/factor_epipolar.h
+++ b/include/core/factor/factor_feature_dummy.h
@@ -5,19 +5,19 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorEpipolar);
+WOLF_PTR_TYPEDEFS(FactorFeatureDummy);
-class FactorEpipolar : public FactorBase
+class FactorFeatureDummy : public FactorBase
{
public:
- FactorEpipolar(const FeatureBasePtr& _feature_ptr,
+ FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorEpipolar() = default;
+ virtual ~FactorFeatureDummy() = default;
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
@@ -50,7 +50,7 @@ class FactorEpipolar : public FactorBase
};
-inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
+inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, FactorStatus _status) :
FactorBase("EPIPOLAR", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
@@ -58,10 +58,10 @@ inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& /*_feature_ptr*/, co
//
}
-inline FactorBasePtr FactorEpipolar::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorFeatureDummy>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
}
} // namespace wolf
diff --git a/include/core/factor/factor_fix_bias.h b/include/core/factor/factor_fix_bias.h
deleted file mode 100644
index 6fb889b55ddda62ad17b0d2644d3cf0ac7044d33..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_fix_bias.h
+++ /dev/null
@@ -1,85 +0,0 @@
-
-#ifndef FACTOR_FIX_BIAS_H_
-#define FACTOR_FIX_BIAS_H_
-
-//Wolf includes
-#include "core/capture/capture_IMU.h"
-#include "core/feature/feature_IMU.h"
-#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(FactorFixBias);
-
-//class
-class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
-{
- public:
- FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
- std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
- {
- // std::cout << "created FactorFixBias " << std::endl;
- }
-
- virtual ~FactorFixBias() = default;
-
- template<typename T>
- bool operator ()(const T* const _ab, const T* const _wb, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T* _residuals) const
-{
- // measurement
- Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
- Eigen::Matrix<T,3,1> ab(_ab);
- Eigen::Matrix<T,3,1> wb(_wb);
-
- // error
- Eigen::Matrix<T,6,1> er;
- er.head(3) = meas.head(3) - ab;
- er.tail(3) = meas.tail(3) - wb;
-
- // residual
- Eigen::Map<Eigen::Matrix<T,6,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
- ////////////////////////////////////////////////////////
- // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
-// using ceres::Jet;
-// Eigen::MatrixXs J(6,6);
-// J.row(0) = ((Jet<Scalar, 3>)(er(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(er(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(er(2))).v;
-// J.row(3) = ((Jet<Scalar, 3>)(er(3))).v;
-// J.row(4) = ((Jet<Scalar, 3>)(er(4))).v;
-// J.row(5) = ((Jet<Scalar, 3>)(er(5))).v;
-
-// J.row(0) = ((Jet<Scalar, 3>)(res(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(res(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
-// J.row(3) = ((Jet<Scalar, 3>)(res(3))).v;
-// J.row(4) = ((Jet<Scalar, 3>)(res(4))).v;
-// J.row(5) = ((Jet<Scalar, 3>)(res(5))).v;
-
-// if (sizeof(er(0)) != sizeof(double))
-// {
-// std::cout << "FactorFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/math/pinhole_tools.h b/include/core/math/pinhole_tools.h
deleted file mode 100644
index ef226651ea81e6bf2645b71069449e364a095a13..0000000000000000000000000000000000000000
--- a/include/core/math/pinhole_tools.h
+++ /dev/null
@@ -1,770 +0,0 @@
-#ifndef PINHOLETOOLS_H
-#define PINHOLETOOLS_H
-
-/**
- * \file pinhole_tools.h
- *
- * \date 06/04/2010
- * \author jsola
- *
- * ## Add a description here ##
- *
- */
-
-#include "core/common/wolf.h"
-
-#include <iostream>
-
-namespace wolf {
-/**
- * Namespace for operations related to the pin-hole model of a camera.
- *
- * The model incorporates radial distortion.
- *
- */
-namespace pinhole {
-
-using Eigen::MatrixBase;
-using Eigen::Matrix;
-
-/**
- * Pin-hole canonical projection.
- * \param _v a 3D point to project
- * \param _up the projected point in the normalized 2D plane
- */
-template<typename Derived1, typename Derived2>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
-
- _up(0) = _v(0) / _v(2);
- _up(1) = _v(1) / _v(2);
-}
-
-/**
- * Pin-hole canonical projection.
- * \return the projected point in the normalized 2D plane
- * \param _v a 3D point
- */
-template<typename Derived>
-inline Matrix<typename Derived::Scalar, 2, 1>
-projectPointToNormalizedPlane(const MatrixBase<Derived>& _v)
-{
- MatrixSizeCheck<3, 1>::check(_v);
-
- typedef typename Derived::Scalar T;
- Matrix<T, 2, 1> up;
-
- projectPointToNormalizedPlane(_v, up);
-
- return up;
-}
-
-/**
- * Pin-hole canonical projection, return also distance (not depth!).
- * \param _v a 3D point
- * \param _up the projected point in the normalized 2D plane
- * \param _dist the distance from the camera to the point
- */
-template<typename Derived1, typename Derived2>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up,
- typename Derived2::Scalar& _dist)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
-
- projectPointToNormalizedPlane(_v, _up);
- _dist = _v.norm();
-}
-
-/**
- * Pin-hole canonical projection, with jacobian
- * \param _v the 3D point to project
- * \param _up the projected 2D point
- * \param _UP_v the Jacibian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up,
- MatrixBase<Derived3>& _UP_v)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
- MatrixSizeCheck<2, 3>::check(_UP_v);
-
- projectPointToNormalizedPlane(_v, _up);
-
- _UP_v(0, 0) = 1.0 / _v(2);
- _UP_v(0, 1) = 0.0;
- _UP_v(0, 2) = -_v(0) / (_v(2) * _v(2));
- _UP_v(1, 0) = 0.0;
- _UP_v(1, 1) = 1.0 / _v(2);
- _UP_v(1, 2) = -_v(1) / (_v(2) * _v(2));
-}
-
-/**
- * Pin-hole canonical projection, with distance (not depth!) and jacobian
- * \param _v the 3D point to project
- * \param _up the projected 2D point
- * \param _UP_v the Jacibian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-inline void
-projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
- MatrixBase<Derived2>& _up,
- typename Derived1::Scalar& _dist,
- MatrixBase<Derived3>& _UP_v)
-{
- MatrixSizeCheck<3, 1>::check(_v);
- MatrixSizeCheck<2, 1>::check(_up);
- MatrixSizeCheck<2, 3>::check(_UP_v);
-
- projectPointToNormalizedPlane(_v, _up, _UP_v);
-
- _dist = _v.norm();
-}
-
-/**
- * Canonical back-projection.
- * \param _u the 2D point in the image plane
- * \param _depth point's depth orthogonal to image plane. Defaults to 1.0
- * \return the back-projected 3D point at the given depth
- */
-template<typename Derived>
-Matrix<typename Derived::Scalar, 3, 1>
-backprojectPointFromNormalizedPlane(const MatrixBase<Derived> & _u,
- const typename Derived::Scalar _depth = 1)
-{
- MatrixSizeCheck<2,1>::check(_u);
-
- Matrix<typename Derived::Scalar, 3, 1> p;
-
- p << _depth*_u ,
- _depth;
-
- return p;
-}
-
-/**
- * Canonical back-projection.
- * \param u the 2D point in the image plane.
- * \param depth point's depth orthogonal to image plane.
- * \param p the 3D point.
- * \param P_u Jacobian of p wrt u.
- * \param P_depth Jacobian of p wrt depth.
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void backprojectPointFromNormalizedPlane(const MatrixBase<Derived1> & u,
- const typename Derived2::Scalar depth,
- MatrixBase<Derived2>& p,
- MatrixBase<Derived3>& P_u,
- MatrixBase<Derived4>& P_depth )
-{
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<3,1>::check(p);
- MatrixSizeCheck<3,2>::check(P_u);
- MatrixSizeCheck<3,1>::check(P_depth);
-
- p = backprojectPointFromNormalizedPlane(u, depth);
-
- P_u(0, 0) = depth;
- P_u(0, 1) = 0.0;
- P_u(1, 0) = 0.0;
- P_u(1, 1) = depth;
- P_u(2, 0) = 0.0;
- P_u(2, 1) = 0.0;
-
- P_depth(0, 0) = u(0);
- P_depth(1, 0) = u(1);
- P_depth(2, 0) = 1.0;
-}
-
-/**
- * Distortion factor for the model s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
- * \param d the distortion parameters vector
- * \param r2 the square of the radius to evaluate, r2 = r^2.
- * \return the distortion factor so that rd = s*r
- */
-template<typename Derived, typename T>
-T distortionFactor(const MatrixBase<Derived> & d,
- T r2)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-
- if (d.size() == 0)
- return (T)1.0;
- T s = (T)1.0;
- T r2i = (T)1.0;
- for (SizeEigen i = 0; i < d.size(); i++)
- { // here we are doing:
- r2i = r2i * r2; // r2i = r^(2*(i+1))
- s += d(i) * r2i; // s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
- }
- /*
- The model is not valid out of the image, and it can bring back landmarks very quickly after they got out.
- We should compute the point where the monotony changes by solving d/du u.f(u,v) = 0, but it would require a lot
- more computations to check the validity of the transform than to compute the distortion... and it should be done
- manually for every size of d (fortunately only 2 or 3 usually).
- So we just make a rough test on s, no camera should have a distortion greater than this, and anyway it will just
- prevent from observing some points on the borders of the image. In that case the landmark will seem to suddenly
- jump outside of the image.
- */
- if (s < 0.6)
- s = (T)1.0;
- return s;
-}
-
-/**
- * Correction factor for the model s = 1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...
- * \param c the correction parameters vector
- * \param r2 the square of the radius to evaluate, r2 = r^2.
- * \return the correction factor so that rc = s*r
- */
-template<typename Derived, typename T>
-T correctionFactor(const MatrixBase<Derived> & c,
- T r2)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
-
- /*
- * Since we use the same polynomial kernel as for the distortion factor, we just call distortionFactor()
- */
- return distortionFactor(c, r2);
-}
-
-/**
- * Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u
- * \param d the distortion parameters vector
- * \param up the point to distort
- * \return the distorted point
- */
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1> & d,
- const MatrixBase<Derived2> & up)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(up);
-
- SizeEigen n = d.size();
- if (n == 0)
- return up;
- else {
- typename Derived2::Scalar r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2
- return distortionFactor(d, r2) * up;
- }
-}
-
-/**
- * Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u, with jacobians
- * \param d the radial distortion parameters vector
- * \param up the point to distort
- * \param ud the distorted point
- * \param UD_up the Jacobian of \a ud wrt \a up
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void distortPoint(const MatrixBase<Derived1> & d,
- const MatrixBase<Derived2> & up,
- MatrixBase<Derived3> & ud,
- MatrixBase<Derived4> & UD_up)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(up);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,2>::check(UD_up);
-
- typedef typename Derived2::Scalar T;
-
- Matrix<T, 2, 1> R2_up;
- Matrix<T, 2, 1> S_up;
-
- SizeEigen n = d.size();
- if (n == 0) {
- ud = up;
- UD_up.setIdentity();
- }
-
- else {
- T r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2
- T s = (T) 1.0;
- T r2i = (T) 1.0;
- T r2im1 = (T) 1.0; //r2*(i-1)
- T S_r2 = (T) 0.0;
-
- for (SizeEigen i = 0; i < n; i++) { //.. here we are doing:
- r2i = r2i * r2; //................. r2i = r^(2*(i+1))
- s += d(i) * r2i; //................ s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
-
- S_r2 = S_r2 + (i + 1) * d(i) * r2im1; //jacobian of s wrt r2 : S_r2 = d_0 + 2 * d1 * r^2 + 3 * d_2 * r^4 + ...
- r2im1 = r2im1 * r2;
- }
-
- if (s < (T)0.6) s = (T)1.0; // because the model is not valid too much out of the image, avoid to wrongly bring them back in the field of view
- // see extensive note in distortionFactor()
- ud = s * up; // finally ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...) * u;
-
- R2_up(0) = 2 * up(0);
- R2_up(1) = 2 * up(1);
-
- S_up(0) = R2_up(0) * S_r2;
- S_up(1) = R2_up(1) * S_r2;
-
- UD_up(0, 0) = S_up(0) * up(0) + s;
- UD_up(0, 1) = S_up(1) * up(0);
- UD_up(1, 0) = S_up(0) * up(1);
- UD_up(1, 1) = S_up(1) * up(1) + s;
- }
-
-}
-
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> undistortPoint(const MatrixBase<Derived1>& c,
- const MatrixBase<Derived2>& ud)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(ud);
-
- SizeEigen n = c.size();
- if (n == 0)
- return ud;
- else {
- typename Derived2::Scalar r2 = ud(0) * ud(0) + ud(1) * ud(1); // this is the norm squared: r2 = ||u||^2
- return correctionFactor(c, r2) * ud;
- }
-}
-
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void undistortPoint(const MatrixBase<Derived1>& c,
- const MatrixBase<Derived2>& ud,
- MatrixBase<Derived3>& up,
- MatrixBase<Derived4>& UP_ud)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,1>::check(up);
- MatrixSizeCheck<2,2>::check(UP_ud);
-
- typedef typename Derived1::Scalar T;
-
- SizeEigen n = c.size();
- Matrix<typename Derived4::Scalar, 2, 1> R2_ud;
- Matrix<typename Derived4::Scalar, 2, 1> S_ud;
-
- if (n == 0)
- {
- up = ud;
- UP_ud.setIdentity();
- }
-
- else
- {
- T r2 = ud(0) * ud(0) + ud(1) * ud(1); // this is the norm squared: r2 = ||u||^2
- T s = (T)1.0;
- T r2i = (T)1.0;
- T r2im1 = (T)1.0; //r2*(i-1)
- T S_r2 = (T)0.0;
-
- for (SizeEigen i = 0; i < n; i++)
- { //.. here we are doing:
- r2i = r2i * r2; //................. r2i = r^(2*(i+1))
- s += c(i) * r2i; //................ s = 1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...
-
- S_r2 = S_r2 + (i + 1) * c(i) * r2im1; //jacobian of s wrt r2 : S_r2 = c_0 + 2 * d1 * r^2 + 3 * c_2 * r^4 + ...
- r2im1 = r2im1 * r2;
- }
- if (s < (T)0.6) s = (T)1.0; // because the model is not valid too much out of the image, avoid to wrongly bring them back in the field of view
- // see extensive note in distortionFactor()
-
- up = s * ud; // finally up = (1 + c_0 * r^2 + c_1 * r^4 + c_2 * r^6 + ...) * u;
-
- R2_ud(0) = 2 * ud(0);
- R2_ud(1) = 2 * ud(1);
-
- S_ud(0) = R2_ud(0) * S_r2;
- S_ud(1) = R2_ud(1) * S_r2;
-
- UP_ud(0, 0) = S_ud(0) * ud(0) + s;
- UP_ud(0, 1) = S_ud(1) * ud(0);
- UP_ud(1, 0) = S_ud(0) * ud(1);
- UP_ud(1, 1) = S_ud(1) * ud(1) + s;
- }
-}
-
-/**
- * Pixellization from k = [u_0, v_0, a_u, a_v]
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param ud the point to pixellize, adimensional
- * \return the point in pixels coordinates
- */
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> pixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& ud)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(ud);
-
- typedef typename Derived2::Scalar T;
-
- T u_0 = k(0);
- T v_0 = k(1);
- T a_u = k(2);
- T a_v = k(3);
- Matrix<T, 2, 1> u;
-
- u(0) = u_0 + a_u * ud(0);
- u(1) = v_0 + a_v * ud(1);
-
- return u;
-}
-
-/**
- * Pixellization from k = [u_0, v_0, a_u, a_v] with jacobians
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param ud the point to pixellize, adimensional
- * \param u the pixellized point
- * \param U_ud the Jacobian of \a u wrt \a ud
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void pixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& ud,
- MatrixBase<Derived3>& u,
- MatrixBase<Derived4>& U_ud)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,2>::check(U_ud);
-
- typedef typename Derived1::Scalar T;
-
- u = pixellizePoint(k, ud);
-
- T a_u = k(2);
- T a_v = k(3);
-
- U_ud(0, 0) = a_u;
- U_ud(0, 1) = 0;
- U_ud(1, 0) = 0;
- U_ud(1, 1) = a_v;
-}
-
-/**
- * Depixellization from k = [u_0, v_0, a_u, a_v]
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param u the point to depixellize, in pixels
- * \return the depixellized point, adimensional
- */
-template<typename Derived1, typename Derived2>
-Matrix<typename Derived2::Scalar, 2, 1> depixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& u)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
-
- typedef typename Derived1::Scalar T;
-
- T u_0 = k(0);
- T v_0 = k(1);
- T a_u = k(2);
- T a_v = k(3);
- Matrix<typename Derived2::Scalar, 2, 1> ud;
-
- ud(0) = (u(0) - u_0) / a_u;
- ud(1) = (u(1) - v_0) / a_v;
-
- return ud;
-}
-
-/**
- * Depixellization from k = [u_0, v_0, a_u, a_v], with Jacobians
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param u the point to depixellize, in pixels
- * \param ud the depixellized point
- * \param UD_u the Jacobian of \a ud wrt \a u
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>
-void depixellizePoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& u,
- MatrixBase<Derived3>& ud,
- MatrixBase<Derived4>& UD_u)
-{
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,1>::check(ud);
- MatrixSizeCheck<2,2>::check(UD_u);
-
- typedef typename Derived1::Scalar T;
- ud = depixellizePoint(k, u);
-
- T a_u = k(2);
- T a_v = k(3);
-
- UD_u(0, 0) = 1.0 / a_u;
- UD_u(0, 1) = 0.0;
- UD_u(1, 0) = 0.0;
- UD_u(1, 1) = 1.0 / a_v;
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \return the projected and distorted point
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-Matrix<typename Derived3::Scalar, 2, 1> projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v)
-{
- MatrixSizeCheck<4,1>::check(k);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
-
- return pixellizePoint( k, distortPoint( d, projectPointToNormalizedPlane( v )));
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \param u the projected and distorted point
- * \param U_v the Jacobian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>
-void projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v,
- MatrixBase<Derived4>& u,
- MatrixBase<Derived5>& U_v)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,3>::check(U_v);
-
- Matrix<typename Derived4::Scalar, 2, 1> up, ud;
- Matrix<typename Derived5::Scalar, 2, 3> UP_v; /// Check this one -> mat23
- Matrix<typename Derived5::Scalar, 2, 2> UD_up, U_ud;
-
- projectPointToNormalizedPlane (v, up, UP_v);
- distortPoint (d, up, ud, UD_up);
- pixellizePoint (k, ud, u, U_ud);
-
- U_v.noalias() = U_ud * UD_up * UP_v;
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion.
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \param u the projected and distorted point
- * \param dist distance from the optical center to the 3D point
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>//, typename T>
-void projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v,
- MatrixBase<Derived4>& u,
- typename Derived3::Scalar& dist)
-{
- MatrixSizeCheck<4,1>::check(k);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
- MatrixSizeCheck<2,1>::check(u);
-
- Matrix<typename Derived4::Scalar, 2, 1> up;
- projectPointToNormalizedPlane(v, up, dist);
- u = pixellizePoint( k, distortPoint( d, up ));
-}
-
-/**
- * Project a point into a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
- * \param u the projected and distorted point
- * \param dist the distance from the camera to the point
- * \param U_v the Jacobian of \a u wrt \a v
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5>
-void projectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& d,
- const MatrixBase<Derived3>& v,
- MatrixBase<Derived4>& u,
- typename Derived3::Scalar& dist,
- MatrixBase<Derived5>& U_v)
-{
- MatrixSizeCheck<4,1>::check(k);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<3,1>::check(v);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<2,3>::check(U_v);
-
- Matrix<typename Derived4::Scalar, 2, 1> up, ud;
- Matrix<typename Derived5::Scalar, 2, 3> UP_v;
- Matrix<typename Derived5::Scalar, 2, 2> UD_up, U_ud;
-
- projectPointToNormalizedPlane (v, up, dist, UP_v);
- distortPoint (d, up, ud, UD_up);
- pixellizePoint (k, ud, u, U_ud);
-
- U_v.noalias() = U_ud * UD_up * UP_v;
-}
-
-/**
- * Back-Project a point from a pin-hole camera with radial distortion
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param c the radial undistortion parameters vector
- * \param u the 2D pixel
- * \param depth the depth prior
- * \return the back-projected 3D point
- */
-template<typename Derived1, typename Derived2, typename Derived3>
-Matrix<typename Derived3::Scalar, 3, 1> backprojectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& c,
- const MatrixBase<Derived3>& u,
- const typename Derived3::Scalar& depth = 1.0)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
-
- return backprojectPointFromNormalizedPlane(undistortPoint(c, depixellizePoint(k, u)), depth);
-}
-
-/**
- * Back-Project a point from a pin-hole camera with radial distortion; give Jacobians
- * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
- * \param c the radial undistortion parameters vector
- * \param u the 2D pixel
- * \param depth the depth prior
- * \param p the back-projected 3D point
- * \param P_u Jacobian of p wrt u
- * \param P_depth Jacobian of p wrt depth
- */
-template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>
-void backprojectPoint(const MatrixBase<Derived1>& k,
- const MatrixBase<Derived2>& c,
- const MatrixBase<Derived3>& u,
- const typename Derived4::Scalar& depth,
- MatrixBase<Derived4>& p,
- MatrixBase<Derived5>& P_u,
- MatrixBase<Derived6>& P_depth)
-{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
- MatrixSizeCheck<4,1>::check(k);
- MatrixSizeCheck<2,1>::check(u);
- MatrixSizeCheck<3,1>::check(p);
- MatrixSizeCheck<3,2>::check(P_u);
- MatrixSizeCheck<3,1>::check(P_depth);
-
- Matrix<typename Derived3::Scalar, 2, 1> up, ud;
- Matrix<typename Derived5::Scalar, 3, 2> P_up;
- Matrix<typename Derived5::Scalar, 2, 2> UP_ud, UD_u;
- depixellizePoint(k, u, ud, UD_u);
- undistortPoint(c, ud, up, UP_ud);
- backprojectPointFromNormalizedPlane(up, depth, p, P_up, P_depth);
-
- P_u.noalias() = P_up * UP_ud * UD_u;
-}
-
-/**
- * Determine if a pixel is inside the region of interest
- * \param pix the pixel to test
- * \param x the region of interest, top-left x
- * \param y the region of interest, top-left y
- * \param width the region of interest width
- * \param height the region of interest height
- */
-template<typename VPix>
-bool isInRoi(const MatrixBase<VPix> & pix, const int x, const int y, const int width, const int height) {
- return ((pix(0) >= x) && (pix(0) <= x + width - 1) && (pix(1) >= y) && (pix(1) <= y + height - 1));
-}
-
-/**
- * Determine if a pixel is inside the image
- * \param pix the pixel to test
- * \param width the image width, in pixels
- * \param height the image height, in pixels
- */
-template<typename VPix>
-bool isInImage(const MatrixBase<VPix> & pix, const int & width, const int & height) {
- return isInRoi(pix, 0, 0, width, height);
-}
-
-/**
- * Compute distortion correction parameters.
- *
- * This method follows the one in Joan Sola's thesis [1], pag 46--49.
- * \param k the intrinsic parameters vector
- * \param d the distortion parameters vector
- * \param c the correction parameters vector. Provide it with the desired size.
- */
-template<class Vk, class Vd, class Vc>
-void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
-
-{
- SizeEigen size = c.size();
-
- if (size != 0)
- {
-
- Scalar r_max = sqrt(k(0) * k(0) / (k(2) * k(2)) + k(1) * k(1) / (k(3) * k(3)));
- Scalar rd_max = 1.2 * r_max;
-
- SizeEigen N_samples = 200; // number of samples
- Scalar iN_samples = 1 / (Scalar)N_samples;
- Scalar rd_n, rc_2, rd_2;
- Eigen::VectorXs rd(N_samples + 1), rc(N_samples + 1);
- Eigen::MatrixXs Rd(N_samples + 1, size);
-
- for (SizeEigen sample = 0; sample <= N_samples; sample++)
- {
-
- rc(sample) = sample * rd_max * iN_samples; // sample * rd_max / N_samples
- rc_2 = rc(sample) * rc(sample);
- rd(sample) = distortionFactor(d, rc_2) * rc(sample);
- rd_2 = rd(sample) * rd(sample);
-
- rd_n = rd(sample); // start with rd
-
- for (SizeEigen order = 0; order < size; order++)
- {
- rd_n *= rd_2; // increment:
- Rd(sample, order) = rd_n; // we have : rd^3, rd^5, rd^7, ...
- }
- }
-
- // solve Rd*c = (rc-rd) for c, with least-squares SVD method:
- // the result is c = pseudo_inv(Rd)*(rc-rd)
- // with pseudo_inv(Rd) = (Rd'*Rd)^-1 * Rd'
-
- // this does not work:
- // jmath::LinearSolvers::solve_Cholesky(Rd, (rc - rd), c);
-
- // therefore we solve manually the pseudo-inverse:
- Eigen::MatrixXs RdtRd(size, size);
- RdtRd = Rd.transpose() * Rd;
- Eigen::MatrixXs iRdtRd(size, size);
- //jmath::ublasExtra::inv(RdtRd, iRdtRd);
- // I understood that iRdtRd is the inverse of RdtRd)
- iRdtRd = RdtRd.inverse();
- Eigen::MatrixXs iRd = iRdtRd * Rd.transpose();
-
- c = iRd * (rc - rd);
- }
-}
-
-} // namespace pinhole
-
-} // namespace wolf
-
-#endif // PINHOLETOOLS_H
diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h
index 58ee217b93f58d5d0230aba2348af004df7937fc..f16aa35f9ab1475ba5c2cc807de3fbb81dffc683 100644
--- a/include/core/processor/processor_tracker_feature_dummy.h
+++ b/include/core/processor/processor_tracker_feature_dummy.h
@@ -10,7 +10,7 @@
#include "core/common/wolf.h"
#include "core/processor/processor_tracker_feature.h"
-#include "core/factor/factor_epipolar.h"
+#include "core/factor/factor_feature_dummy.h"
namespace wolf
{
diff --git a/include/core/state_block/local_parametrization_polyline_extreme.h b/include/core/state_block/local_parametrization_polyline_extreme.h
deleted file mode 100644
index ce025e7404b0cde7f67a3521255b3f6597302b36..0000000000000000000000000000000000000000
--- a/include/core/state_block/local_parametrization_polyline_extreme.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * \file local_parametrization_polyline_extreme.h
- *
- * Created on: Jun 10, 2016
- * \author: jvallve
- */
-
-#ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
-#define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
-
-#include "core/state_block/local_parametrization_base.h"
-
-namespace wolf {
-
-/**
- * \brief Local parametrization for polylines not defined extreme points
- *
- */
-class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
-{
- private:
- StateBlockPtr reference_point_;
- public:
- LocalParametrizationPolylineExtreme(StateBlockPtr _reference_point);
- virtual ~LocalParametrizationPolylineExtreme();
-
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta);
-};
-
-} // namespace wolf
-
-#endif /* LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_ */
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index 0502e8acc78e1efec5c01bbdfb392ccaf21aa1dd..c6776995796c55b82a70acd396ea0dca63237190 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -83,7 +83,7 @@ FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature
WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
, " with origin feature " , _feature_other_ptr->id() );
- auto ctr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
+ auto ctr = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
return ctr;
}
diff --git a/src/state_block/local_parametrization_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp
deleted file mode 100644
index 5077eb0c7e2bfa70949e66244de77292e42f8aa4..0000000000000000000000000000000000000000
--- a/src/state_block/local_parametrization_polyline_extreme.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-#include "core/state_block/local_parametrization_polyline_extreme.h"
-#include "core/state_block/state_block.h"
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-LocalParametrizationPolylineExtreme::LocalParametrizationPolylineExtreme(StateBlockPtr _reference_point) :
- LocalParametrizationBase(2, 1),
- reference_point_(_reference_point)
-{
-}
-
-LocalParametrizationPolylineExtreme::~LocalParametrizationPolylineExtreme()
-{
-}
-
-bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const
-{
-
- assert(_point.size() == global_size_ && "Wrong size of input point.");
- assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
- assert(_point_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
-
- _point_plus_delta_theta = reference_point_->getState().head(2) + Eigen::Rotation2Ds(_delta_theta(0)) * (_point - reference_point_->getState().head(2));
-
- return true;
-}
-
-bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
-{
- assert(_point.size() == global_size_ && "Wrong size of input point.");
- assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
-
- _jacobian(0,0) = reference_point_->getState()(1) - _point(1);
- _jacobian(1,0) = _point(0) - reference_point_->getState()(0);
-
- return true;
-}
-
-bool LocalParametrizationPolylineExtreme::minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta)
-{
- Eigen::Vector2s v1 = _point1 - reference_point_->getState().head(2);
- Eigen::Vector2s v2 = _point2 - reference_point_->getState().head(2);
-
- _delta_theta(0) = pi2pi(atan2(v2(1),v2(0)) - atan2(v1(1),v1(0)));
-
- return true;
-}
-
-} // namespace wolf
-
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8c05dd188b102bc77eda5519c785be5f7e048b88..c0d47dea04b20e26ed26cf685b68f169a073bdab 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -93,7 +93,6 @@ wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
target_link_libraries(gtest_parser_yaml ${PROJECT_NAME})
# Problem class test
-# TODO: TO BE FIXED
wolf_add_gtest(gtest_problem gtest_problem.cpp)
target_link_libraries(gtest_problem ${PROJECT_NAME})
@@ -172,9 +171,6 @@ target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME})
-# Pinhole test
-wolf_add_gtest(gtest_pinhole gtest_pinhole.cpp)
-target_link_libraries(gtest_pinhole ${PROJECT_NAME})
# ProcessorFrameNearestNeighborFilter class test
wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
deleted file mode 100644
index 376f15c10f3713b166e78e33feff6521c268480f..0000000000000000000000000000000000000000
--- a/test/gtest_pinhole.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/*
- * gtest_pinhole.cpp
- *
- * Created on: Nov 27, 2016
- * Author: jsola
- */
-
-#include "core/math/pinhole_tools.h"
-#include "utils_gtest.h"
-
-using namespace Eigen;
-using namespace wolf;
-using namespace pinhole;
-using Constants::EPS;
-using Constants::EPS_SMALL;
-
-TEST(Pinhole, EigenTypes)
-{
- Vector3s vs; vs << 1,2,4;
- Vector2s ps;
- Map<Vector3s> vm(vs.data());
- Map<Vector2s> pm(ps.data());
- Map<const Vector3s> cvm(vs.data());
- VectorXs vd(3); vd = vs;
- VectorXs pd(2);
-
- Vector2s pe; pe << 0.25, 0.5; // expected result
-
- // static size
- projectPointToNormalizedPlane(vs,ps);
- ASSERT_TRUE((ps - pe).isMuchSmallerThan(1, EPS_SMALL));
-
- // dynamic size
- projectPointToNormalizedPlane(vd,pd);
- ASSERT_TRUE((pd - pe).isMuchSmallerThan(1, EPS_SMALL));
-
- // Map
- projectPointToNormalizedPlane(vm,pm);
- ASSERT_TRUE((pm - pe).isMuchSmallerThan(1, EPS_SMALL));
-
- // Map const
- projectPointToNormalizedPlane(cvm,pm);
- ASSERT_TRUE((pm - pe).isMuchSmallerThan(1, EPS_SMALL));
-}
-
-TEST(Pinhole, pix_pnt_pix)
-{
- Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example
- Vector2s d; d << -0.301701, 0.0963189;
- Vector2s u0;
- Vector3s p;
- Vector2s u1;
- Vector2s c; // should be close to (0.297923, 0.216263)
- Scalar depth = 10;
- Scalar pix_error_allowed = 0.1;
-
- computeCorrectionModel(k, d, c);
-
- WOLF_DEBUG("correction: ", c.transpose(), " // should be close to (0.297923, 0.216263)");
-
- // choose some random points
- u0 << 123, 321;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-
- u0 << 1, 1;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-
- u0 << 320, 240;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-
- u0 << 640, 480;
- p = backprojectPoint(k, c, u0, depth);
- u1 = projectPoint(k, d, p);
- WOLF_DEBUG("error: ", (u1-u0).transpose());
- ASSERT_DOUBLE_EQ(p(2), depth);
- ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose();
-}
-
-TEST(Pinhole, Jacobians)
-{
- Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example
- Vector2s d; d << -0.301701, 0.0963189;
- Vector3s v;
- Vector2s u;
- MatrixXs U_v(2, 3);
-
- v << 1,2,4;
- projectPoint(k, d, v, u, U_v);
-
- Vector2s c;
- computeCorrectionModel(k, d, c);
-
- Vector3s p;
- MatrixXs P_u(3,2), P_depth(3,1);
- backprojectPoint(k, c, u, 4, p, P_u, P_depth);
-
- // check that reprojected point is close to original
- ASSERT_LT((p-v).norm(), 1e-3) << "p: " << p.transpose() << "\nv: " << v.transpose();
-
- // Check that both Jacobians are inverse one another (in the smallest dimension)
- ASSERT_TRUE((U_v*P_u - Matrix2s::Identity()).isMuchSmallerThan(1, 1e-3)) << "U_v*P_u: " << U_v*P_u;
-
- WOLF_DEBUG("U_v*P_u: \n", U_v*P_u);
- WOLF_DEBUG("P_u*U_v: \n", P_u*U_v);
-}
-
-TEST(Pinhole, JacobiansDist)
-{
- Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example
- Vector2s d; d << -0.301701, 0.0963189;
- Vector3s v;
- Vector2s u;
- MatrixXs U_v(2, 3);
- Scalar dist;
-
- v << 1,2,4;
- projectPoint(k, d, v, u, dist, U_v);
-
- // check that the recovered distance is indeed the distance to v
- ASSERT_DOUBLE_EQ(dist, v.norm());
-
- Vector2s c;
- computeCorrectionModel(k, d, c);
-
- Vector3s p;
- MatrixXs P_u(3,2), P_depth(3,1);
- backprojectPoint(k, c, u, 4, p, P_u, P_depth);
-
- // check that reprojected point is close to original
- ASSERT_LT((p-v).norm(), 1e-3) << "p: " << p.transpose() << "\nv: " << v.transpose();
-
- // Check that both Jacobians are inverse one another (in the smallest dimension)
- ASSERT_TRUE((U_v*P_u - Matrix2s::Identity()).isMuchSmallerThan(1, 1e-3)) << "U_v*P_u: " << U_v*P_u;
-}
-
-TEST(Pinhole, isInRoi)
-{
- Vector2s p;
- p << 15, 15;
-
- ASSERT_TRUE (isInRoi(p, 10, 10, 10, 10));
- ASSERT_FALSE(isInRoi(p, 0, 0, 10, 10));
- ASSERT_FALSE(isInRoi(p, 0, 10, 10, 10));
- ASSERT_FALSE(isInRoi(p, 0, 20, 10, 10));
- ASSERT_FALSE(isInRoi(p, 10, 0, 10, 10));
- ASSERT_FALSE(isInRoi(p, 10, 20, 10, 10));
- ASSERT_FALSE(isInRoi(p, 20, 0, 10, 10));
- ASSERT_FALSE(isInRoi(p, 20, 10, 10, 10));
- ASSERT_FALSE(isInRoi(p, 20, 20, 10, 10));
-}
-
-TEST(Pinhole, isInImage)
-{
- Vector2s p;
- p << 15, 15;
- ASSERT_TRUE (isInImage(p, 640, 480));
-
- p << -15, -15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << -15, 15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << -15, 500;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 15, -15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 15, 500;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 700, -15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 700, 15;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
- p << 700, 500;
- ASSERT_FALSE (isInImage(p, 640, 480));
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-