Closing #203 + some spring cleaning

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
   )

include/core/factor/factor_epipolar.h → 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

include/core/factor/factor_fix_bias.h

-
-#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

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
 {

include/core/state_block/local_parametrization_polyline_extreme.h

-/*
- * \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_ */

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;
 }

src/state_block/local_parametrization_polyline_extreme.cpp

-#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
-

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)

test/gtest_pinhole.cpp

-/*
- * 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();
-}
-