From c6dd6e2f9143a5a859a6e62907cece140e81bf95 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Tue, 30 Jan 2018 09:15:18 +0100
Subject: [PATCH] add algorithm residualtrilinearity
---
src/CMakeLists.txt | 8 +-
.../alg_residualtrilinearity.cpp | 108 ++++++++++++++++++
.../alg_residualtrilinearity.h | 98 ++++++++++++++++
.../alg_residualtrilinearity_load_yaml.cpp | 44 +++++++
src/common_class/trifocaltensor.cpp | 23 ++--
src/common_class/trifocaltensor.h | 5 +
6 files changed, 273 insertions(+), 13 deletions(-)
create mode 100644 src/algorithms/residualtrilinearity/alg_residualtrilinearity.cpp
create mode 100644 src/algorithms/residualtrilinearity/alg_residualtrilinearity.h
create mode 100644 src/algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 6d93eae..6b6149d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -89,7 +89,9 @@ SET(sources
algorithms/trackfeatures/alg_trackfeatures.cpp
algorithms/trackfeatures/alg_trackfeatures_load_yaml.cpp
algorithms/activesearch/alg_activesearch.cpp
- algorithms/activesearch/alg_activesearch_load_yaml.cpp)
+ algorithms/activesearch/alg_activesearch_load_yaml.cpp
+ algorithms/residualtrilinearity/alg_residualtrilinearity.cpp
+ algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp)
# application header files
SET(headers_main
@@ -186,6 +188,9 @@ SET(headers_alg_trackfeatures
algorithms/trackfeatures/alg_trackfeatures.h)
SET(headers_alg_activesearch
algorithms/activesearch/alg_activesearch.h)
+SET(headers_alg_residualtrilinearity
+ algorithms/residualtrilinearity/alg_residualtrilinearity.h)
+
# locate the necessary dependencies
FIND_PACKAGE(Eigen3 REQUIRED)
@@ -297,6 +302,7 @@ INSTALL(FILES ${headers_alg} DESTINATION include/${PROJECT_NAME}/algorithms)
INSTALL(FILES ${headers_alg_opticalflowpyrlk} DESTINATION include/${PROJECT_NAME}/algorithms/opticalflowpyrlk)
INSTALL(FILES ${headers_alg_trackfeatures} DESTINATION include/${PROJECT_NAME}/algorithms/trackfeatures)
INSTALL(FILES ${headers_alg_activesearch} DESTINATION include/${PROJECT_NAME}/algorithms/activesearch)
+INSTALL(FILES ${headers_alg_residualtrilinearity} DESTINATION include/${PROJECT_NAME}/algorithms/residualtrilinearity)
INSTALL(FILES ../cmake_modules/Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES "${VU_CONFIG_DIR}/config.h" DESTINATION include/${PROJECT_NAME}/_internal)
diff --git a/src/algorithms/residualtrilinearity/alg_residualtrilinearity.cpp b/src/algorithms/residualtrilinearity/alg_residualtrilinearity.cpp
new file mode 100644
index 0000000..3f40bd0
--- /dev/null
+++ b/src/algorithms/residualtrilinearity/alg_residualtrilinearity.cpp
@@ -0,0 +1,108 @@
+#include "alg_residualtrilinearity.h"
+
+namespace vision_utils {
+
+AlgorithmRESIDUALTRILINEARITY::AlgorithmRESIDUALTRILINEARITY(void)
+{
+ initialized_=false;
+}
+
+AlgorithmRESIDUALTRILINEARITY::~AlgorithmRESIDUALTRILINEARITY(void)
+{
+}
+
+void AlgorithmRESIDUALTRILINEARITY::initAlg(void)
+{
+ // Relative transforms between cameras
+ c2Ec1_ = Eigen::Matrix3d::Identity();
+ c3Ec2_ = Eigen::Matrix3d::Identity();
+
+ Eigen::MatrixXd P1(3,4), P2(3,4), P3(3,4);
+ P1.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
+ P1.block(0,3,3,1) = Eigen::MatrixXd::Zero(3,1);
+ P2 = P1;
+ P3 = P1;
+ trifocaltensor_.computeTensorFromProjectionMat(P1, P2, P3);
+
+ initialized_ = true;
+}
+
+bool AlgorithmRESIDUALTRILINEARITY::getResidualEpipolarLinesAndPLP(const Eigen::Matrix3d& wRc1, const Eigen::MatrixXd& wtc1, const Eigen::Matrix3d& wRc2, const Eigen::MatrixXd& wtc2, const Eigen::Matrix3d& wRc3, const Eigen::MatrixXd& wtc3, const Eigen::MatrixXd& feat, const Eigen::MatrixXd& feat_cov, Eigen::MatrixXd& res, Eigen::MatrixXd& res_cov, const bool& state_new)
+{
+ // Compute Tensor and apipolar lines if state has changed
+ if (state_new)
+ {
+ // Relative transforms between cameras
+ Eigen::Matrix3d c1Rc2, c1Rc3, c2Rc3;
+ Eigen::MatrixXd c1tc2(3,1), c1tc3(3,1), c2tc3(3,1);
+ c1Rc2 = wRc1.transpose() * wRc2;
+ c1Rc3 = wRc1.transpose() * wRc3;
+ c2Rc3 = wRc2.transpose() * wRc3;
+ c1tc2 = wRc1.transpose() * (wtc2 - wtc1);
+ c1tc3 = wRc1.transpose() * (wtc3 - wtc1);
+ c2tc3 = wRc2.transpose() * (wtc3 - wtc2);
+
+ // Projection matrices (canonic cameras)
+ Eigen::MatrixXd P1(3,4), P2(3,4), P3(3,4);
+ P1.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
+ P1.block(0,3,3,1) = Eigen::MatrixXd::Zero(3,1);
+ P2.block(0,0,3,3) = c1Rc2.transpose();
+ P2.block(0,3,3,1) = -c1Rc2.transpose()*c1tc2;
+ P3.block(0,0,3,3) = c1Rc3.transpose();
+ P3.block(0,3,3,1) = -c1Rc3.transpose()*c1tc3;
+
+ // Essential Matrices c2c1 and c3c2
+ c2Ec1_ = c1Rc2.transpose()*skew(c1tc2);
+ c3Ec2_ = c2Rc3.transpose()*skew(c2tc3);
+
+ // Trifocal tensor
+ trifocaltensor_.computeTensorFromProjectionMat(P1, P2, P3);
+ }
+
+ // Epipolar line in c2
+ Eigen::MatrixXd c2l_can = c2Ec1_*feat.col(0);
+
+ // Line in c2 perpendicular to epipolar from c1
+ Eigen::MatrixXd c2lp_can(3,1);
+ c2lp_can(0,0) = c2l_can(1);
+ c2lp_can(1,0) = -c2l_can(0);
+ c2lp_can(2,0) = -feat(0)*c2l_can(1)+feat(1)*c2l_can(0);
+
+ // Tensor elements
+ Eigen::Matrix3d T1,T2,T3;
+ trifocaltensor_.getLayers(T1,T2,T3);
+ Eigen::Matrix3d xTi = Eigen::Matrix3d::Zero(3,3);
+ xTi.col(0) = T1*feat.col(0);
+ xTi.col(1) = T2*feat.col(0);
+ xTi.col(2) = T3*feat.col(0);
+
+ // 1st constraint: Epipolar line c1c2
+ Eigen::MatrixXd residual(4,1);
+ residual(0,0) = feat.col(1).transpose()*c2Ec1_*feat.col(0);
+
+ // 2nd constraint: Epipolar line c1c2
+ residual(1,0) = feat.col(2).transpose()*c3Ec2_*feat.col(1);
+
+ // 3rd constraint: Point-point-line
+ Eigen::MatrixXd obs_model = (c2lp_can.transpose()*xTi).transpose();
+ obs_model = obs_model/obs_model(2,0);
+ residual.block(2,0,2,1) = obs_model-feat.col(2);
+
+ // TODO: Implement Covariance from jacobians
+ // Now feat_cov is considered canonic
+ // We consider a 2 pixel error for the epipolar lines.
+ res_cov = Eigen::MatrixXd::Identity(4,4);
+ res_cov(0,0) = 2;
+ res_cov(1,1) = 2;
+ res_cov.block(2,2,2,2) = 1.7321*feat_cov.block(0,0,2,2);
+
+ return true;
+}
+
+} /* namespace vision_utils */
+
+// Register in the AlgorithmsFactory
+namespace vision_utils
+{
+VU_REGISTER_ALGORITHM("RESIDUALTRILINEARITY", AlgorithmRESIDUALTRILINEARITY);
+} /* namespace vision_utils */
diff --git a/src/algorithms/residualtrilinearity/alg_residualtrilinearity.h b/src/algorithms/residualtrilinearity/alg_residualtrilinearity.h
new file mode 100644
index 0000000..a5695b5
--- /dev/null
+++ b/src/algorithms/residualtrilinearity/alg_residualtrilinearity.h
@@ -0,0 +1,98 @@
+#ifndef _ALGORITHM_RESIDUALTRILINEARITY_H_
+#define _ALGORITHM_RESIDUALTRILINEARITY_H_
+
+#include "../../vision_utils.h"
+#include "../algorithm_base.h"
+#include "../algorithm_factory.h"
+#include "../../common_class/rotations.h"
+#include "../../common_class/trifocaltensor.h"
+
+namespace vision_utils {
+
+// Create all pointers
+VU_PTR_TYPEDEFS(AlgorithmRESIDUALTRILINEARITY);
+VU_PTR_TYPEDEFS(AlgorithmParamsRESIDUALTRILINEARITY);
+
+/** \brief Class parameters
+ *
+ */
+struct AlgorithmParamsRESIDUALTRILINEARITY: public AlgorithmParamsBase
+{
+ ~AlgorithmParamsRESIDUALTRILINEARITY(){}
+};
+
+class AlgorithmRESIDUALTRILINEARITY : public AlgorithmBase {
+
+ public:
+
+ AlgorithmRESIDUALTRILINEARITY();
+ virtual ~AlgorithmRESIDUALTRILINEARITY(void);
+
+ /**
+ * \brief Initalize internal variables
+ */
+ void initAlg(void);
+
+ // Factory method
+ static AlgorithmBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);
+
+ AlgorithmParamsRESIDUALTRILINEARITYPtr getParams(void);
+
+ void setParams(AlgorithmParamsRESIDUALTRILINEARITYPtr _params);
+
+ /**
+ * \brief Get residual considering the constraint formed by the two epipolar lines (3 frames) and the trilinearity Point-Line-Point
+ *
+ * The feature (feat) is considered to be canonic, i.e. feat = (K^-1*m) where m is a feature in the image plane.
+ * Similarly, the Covariance feat_cov is also canonic, i.e. feat_cov = (K^-1*m_cov*K^-T) where m_cov is the covariance of a point in image plane.
+ */
+ bool getResidualEpipolarLinesAndPLP(const Eigen::Matrix3d& wRc1, const Eigen::MatrixXd& wtc1, const Eigen::Matrix3d& wRc2, const Eigen::MatrixXd& wtc2, const Eigen::Matrix3d& wRc3, const Eigen::MatrixXd& wtc3, const Eigen::MatrixXd& feat, const Eigen::MatrixXd& feat_cov, Eigen::MatrixXd& res, Eigen::MatrixXd& res_cov, const bool& state_new = false);
+
+ private:
+
+ // Essential matrices
+ Eigen::Matrix3d c2Ec1_, c3Ec2_;
+
+ TrifocalTensor trifocaltensor_;
+
+ AlgorithmParamsRESIDUALTRILINEARITYPtr params_ptr_;
+
+ /**
+ * \brief Define the algorithm
+ */
+ void defineAlgorithm(const ParamsBasePtr _params);
+};
+
+/*
+ * brief Retrieve object parameters
+ */
+inline AlgorithmParamsRESIDUALTRILINEARITYPtr AlgorithmRESIDUALTRILINEARITY::getParams(void) { return params_ptr_; }
+
+/*
+ * brief Set object parameters
+ */
+inline void AlgorithmRESIDUALTRILINEARITY::setParams(AlgorithmParamsRESIDUALTRILINEARITYPtr _params)
+{ params_ptr_ = _params; }
+
+/*
+ * brief Define detector
+ */
+inline void AlgorithmRESIDUALTRILINEARITY::defineAlgorithm(const ParamsBasePtr _params)
+{
+ params_ptr_ = std::static_pointer_cast<AlgorithmParamsRESIDUALTRILINEARITY>(_params);
+}
+
+/*
+ * brief Create object in factory
+ */
+inline AlgorithmBasePtr AlgorithmRESIDUALTRILINEARITY::create(const std::string& _unique_name, const ParamsBasePtr _params)
+{
+ AlgorithmRESIDUALTRILINEARITYPtr alg_ptr = std::make_shared<AlgorithmRESIDUALTRILINEARITY>();
+ alg_ptr->setName(_unique_name);
+ alg_ptr->defineAlgorithm(_params);
+ return alg_ptr;
+}
+
+} /* namespace vision_utils */
+
+#endif /* _ALGORITHM_RESIDUALTRILINEARITY_H_ */
diff --git a/src/algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp b/src/algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp
new file mode 100644
index 0000000..c2d296a
--- /dev/null
+++ b/src/algorithms/residualtrilinearity/alg_residualtrilinearity_load_yaml.cpp
@@ -0,0 +1,44 @@
+#include "alg_residualtrilinearity.h"
+
+#ifdef USING_YAML
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace vision_utils
+{
+
+namespace
+{
+
+static ParamsBasePtr createParamsRESIDUALTRILINEARITYAlgorithm(const std::string & _filename_dot_yaml)
+{
+ AlgorithmParamsRESIDUALTRILINEARITYPtr params_ptr = std::make_shared<AlgorithmParamsRESIDUALTRILINEARITY>();
+
+ using std::string;
+ using YAML::Node;
+ Node yaml_params = YAML::LoadFile(_filename_dot_yaml);
+ if (!yaml_params.IsNull())
+ {
+ Node d_yaml = yaml_params["algorithm"];
+ if(d_yaml["type"].as<string>() == "RESIDUALTRILINEARITY")
+ {
+ }else
+ {
+ std::cerr << "Bad configuration file. Wrong type " << d_yaml["type"].as<string>() << std::endl;
+ return nullptr;
+ }
+ }
+
+ return params_ptr;
+}
+
+// Register in the SensorFactory
+const bool VU_UNUSED registered_matRESIDUALTRILINEARITY_params = ParamsFactory::get().registerCreator("RESIDUALTRILINEARITY ALG", createParamsRESIDUALTRILINEARITYAlgorithm);
+
+} /* namespace [unnamed] */
+
+} /* namespace vision_utils */
+
+#endif /* IF USING_YAML */
+
diff --git a/src/common_class/trifocaltensor.cpp b/src/common_class/trifocaltensor.cpp
index 011e910..5d840b7 100644
--- a/src/common_class/trifocaltensor.cpp
+++ b/src/common_class/trifocaltensor.cpp
@@ -12,10 +12,7 @@ TrifocalTensor::TrifocalTensor():
max_reprojection_error_(-1),
percentage_of_correct_reprojected_points_(-1)
{
- for(size_t kk = 0; kk<3; ++kk)
- for(size_t ii = 0; ii<3; ++ii)
- for(size_t jj = 0; jj<3; ++jj)
- tensor_[ii][jj][kk] = 0;
+ iniClassVars();
}
TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points):
@@ -28,10 +25,7 @@ TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_repr
if (percentage_of_correct_reprojected_points_<0.0 || percentage_of_correct_reprojected_points_>1.0)
VU_ERROR("Incorrect parameter: percentage_of_correct_reprojected_points. Values must be between 0~1");
- for(size_t kk = 0; kk<3; ++kk)
- for(size_t ii = 0; ii<3; ++ii)
- for(size_t jj = 0; jj<3; ++jj)
- tensor_[ii][jj][kk] = 0;
+ iniClassVars();
}
TrifocalTensor::TrifocalTensor(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& P3):
@@ -41,10 +35,7 @@ TrifocalTensor::TrifocalTensor(const Eigen::MatrixXd& P1, const Eigen::MatrixXd&
max_reprojection_error_(-1),
percentage_of_correct_reprojected_points_(-1)
{
- for(size_t kk = 0; kk<3; ++kk)
- for(size_t ii = 0; ii<3; ++ii)
- for(size_t jj = 0; jj<3; ++jj)
- tensor_[ii][jj][kk] = 0;
+ iniClassVars();
computeTensorFromProjectionMat(P1,P2,P3);
}
@@ -54,6 +45,14 @@ TrifocalTensor::~TrifocalTensor()
{
}
+void TrifocalTensor::iniClassVars(void)
+{
+ for(size_t kk = 0; kk<3; ++kk)
+ for(size_t ii = 0; ii<3; ++ii)
+ for(size_t jj = 0; jj<3; ++jj)
+ tensor_[ii][jj][kk] = 0;
+}
+
float TrifocalTensor::operator()(size_t row, size_t col, size_t depth)
{
return tensor_[row][col][depth];
diff --git a/src/common_class/trifocaltensor.h b/src/common_class/trifocaltensor.h
index 7100d22..2037b88 100644
--- a/src/common_class/trifocaltensor.h
+++ b/src/common_class/trifocaltensor.h
@@ -71,6 +71,11 @@ private:
double percentage_of_correct_reprojected_points_; // Percentage of correct reprojected points to consider the tensor correct
+ /**
+ * \brief Initialize Class variables
+ */
+ void iniClassVars(void);
+
/**
* \brief Overloading () for a easy acces to the elements of the tensor
*/
--
GitLab