Skip to content
Snippets Groups Projects
Commit 455bada2 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

Templatize call and tensor class object. Create TrifocalTensorBase to set... 

Templatize call and tensor class object. Create TrifocalTensorBase to set default tensor object type.
parent c51256cf
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/common_class/trifocaltensor.h
+ 72
− 48
View file @ 455bada2
......@@ -10,12 +10,11 @@
namespace vision_utils {
VU_PTR_TYPEDEFS(TrifocalTensor);
/**
* \brief TrifocalTensor class
* \brief TrifocalTensorBase class
*/
class TrifocalTensor {
template<typename Derived = Scalar>
class TrifocalTensorBase {
public:
double comp_time_; // Detection time
......@@ -23,22 +22,22 @@ public:
/**
* \brief Constructor
*/
TrifocalTensor();
TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points);
template<typename D2>
TrifocalTensor(const Eigen::MatrixBase<D2>& P1, const Eigen::MatrixBase<D2>& P2, const Eigen::MatrixBase<D2>& P3);
TrifocalTensorBase();
TrifocalTensorBase(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points);
template<typename Derived2>
TrifocalTensorBase(const Eigen::MatrixBase<Derived2>& P1, const Eigen::MatrixBase<Derived2>& P2, const Eigen::MatrixBase<Derived2>& P3);
/**
* \brief Destructor
*/
virtual ~TrifocalTensor();
virtual ~TrifocalTensorBase();
/**
* \brief Compute trifocal tensor
*/
bool computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
template<typename D2>
bool computeTensorFromProjectionMat(const Eigen::MatrixBase<D2>& P1, const Eigen::MatrixBase<D2>& P2, const Eigen::MatrixBase<D2>& P3);
template<typename Derived2>
bool computeTensorFromProjectionMat(const Eigen::MatrixBase<Derived2>& P1, const Eigen::MatrixBase<Derived2>& P2, const Eigen::MatrixBase<Derived2>& P3);
bool computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
bool computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3);
......@@ -61,7 +60,7 @@ public:
private:
float tensor_[3][3][3]; // Main Tensor matrix (3x3x3)
Derived tensor_[3][3][3]; // Main Tensor matrix (3x3x3)
int max_iterations_; // Maximum number of iterations when sampling the candidate points (matched in three frames)
......@@ -115,12 +114,14 @@ private:
void normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3, Eigen::Matrix3d& T_norm);
};
inline TrifocalTensor::TrifocalTensor()
template<typename Derived>
inline TrifocalTensorBase<Derived>::TrifocalTensorBase()
{
iniClassVars(-1,-1,-1);
}
inline TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points)
template<typename Derived>
inline TrifocalTensorBase<Derived>::TrifocalTensorBase(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points)
{
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");
......@@ -128,8 +129,9 @@ inline TrifocalTensor::TrifocalTensor(const int& max_iterations, const double& m
iniClassVars(max_iterations,max_reprojection_error,percentage_of_correct_reprojected_points);
}
template<typename D2>
inline TrifocalTensor::TrifocalTensor(const Eigen::MatrixBase<D2>& P1, const Eigen::MatrixBase<D2>& P2, const Eigen::MatrixBase<D2>& P3)
template<typename Derived>
template<typename Derived2>
inline TrifocalTensorBase<Derived>::TrifocalTensorBase(const Eigen::MatrixBase<Derived2>& P1, const Eigen::MatrixBase<Derived2>& P2, const Eigen::MatrixBase<Derived2>& P3)
{
MatrixSizeCheck<3,4>::check(P1);
MatrixSizeCheck<3,4>::check(P2);
......@@ -140,11 +142,13 @@ inline TrifocalTensor::TrifocalTensor(const Eigen::MatrixBase<D2>& P1, const Eig
computeTensorFromProjectionMat(P1,P2,P3);
}
inline TrifocalTensor::~TrifocalTensor()
template<typename Derived>
inline TrifocalTensorBase<Derived>::~TrifocalTensorBase()
{
}
inline void TrifocalTensor::iniClassVars(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points)
template<typename Derived>
inline void TrifocalTensorBase<Derived>::iniClassVars(const int& max_iterations, const double& max_reprojection_error, const double& percentage_of_correct_reprojected_points)
{
comp_time_ = 0.0;
max_iterations_ = max_iterations;
......@@ -157,12 +161,14 @@ inline void TrifocalTensor::iniClassVars(const int& max_iterations, const double
tensor_[ii][jj][kk] = 0;
}
inline float TrifocalTensor::operator()(size_t row, size_t col, size_t depth)
template<typename Derived>
inline float TrifocalTensorBase<Derived>::operator()(size_t row, size_t col, size_t depth)
{
return tensor_[row][col][depth];
}
inline void TrifocalTensor::print() const
template<typename Derived>
inline void TrifocalTensorBase<Derived>::print() const
{
std::cout << "Trifocal tensor : " << std::endl;
for(size_t ii = 0; ii<3; ++ii)
......@@ -173,7 +179,8 @@ inline void TrifocalTensor::print() const
}
}
inline void TrifocalTensor::fillTensor(const Eigen::VectorXd& tensorVector)
template<typename Derived>
inline void TrifocalTensorBase<Derived>::fillTensor(const Eigen::VectorXd& tensorVector)
{
for(size_t kk = 0; kk<3; ++kk) {
for(size_t ii = 0; ii<3; ++ii) {
......@@ -184,7 +191,8 @@ inline void TrifocalTensor::fillTensor(const Eigen::VectorXd& tensorVector)
}
}
inline void TrifocalTensor::getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2, Eigen::Matrix3d& T3)
template<typename Derived>
inline void TrifocalTensorBase<Derived>::getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2, Eigen::Matrix3d& T3)
{
for(size_t ii = 0; ii<3; ++ii)
{
......@@ -197,7 +205,8 @@ inline void TrifocalTensor::getLayers(Eigen::Matrix3d& T1, Eigen::Matrix3d& T2,
}
}
inline Eigen::VectorXd TrifocalTensor::getRandomCandidates(const int& min_points, const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3)
template<typename Derived>
inline Eigen::VectorXd TrifocalTensorBase<Derived>::getRandomCandidates(const int& min_points, const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3)
{
std::random_device rd; //Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
......@@ -232,7 +241,8 @@ inline Eigen::VectorXd TrifocalTensor::getRandomCandidates(const int& min_points
return idx;
}
inline double TrifocalTensor::getErrorTrifocal(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
template<typename Derived>
inline double TrifocalTensorBase<Derived>::getErrorTrifocal(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
{
double count = 0.0;
for (int ii=0; ii<list1.rows(); ++ii)
......@@ -251,7 +261,8 @@ inline double TrifocalTensor::getErrorTrifocal(const Eigen::MatrixXd& list1, con
return count/list1.rows();
}
inline bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
template<typename Derived>
inline bool TrifocalTensorBase<Derived>::computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
{
clock_t tStart = clock();
......@@ -282,13 +293,14 @@ inline bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Ei
}
template<typename Derived>
inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixBase<Derived>& P1, const Eigen::MatrixBase<Derived>& P2, const Eigen::MatrixBase<Derived>& P3)
template<typename Derived2>
inline bool TrifocalTensorBase<Derived>::computeTensorFromProjectionMat(const Eigen::MatrixBase<Derived2>& P1, const Eigen::MatrixBase<Derived2>& P2, const Eigen::MatrixBase<Derived2>& P3)
{
MatrixSizeCheck<3,4>::check(P1);
MatrixSizeCheck<3,4>::check(P2);
MatrixSizeCheck<3,4>::check(P3);
Eigen::MatrixXd c2Pc1(3,4), c3Pc1(3,4);
Eigen::Matrix<Derived, 3, 4> c2Pc1, c3Pc1;
if(P1.block(0,0,3,3).isIdentity(EPS) && P1.block(0,3,3,1).isZero(EPS))
{
c2Pc1 = P2;
......@@ -296,17 +308,17 @@ inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixBa
}
else
{
Eigen::Matrix3d wRc1 = P1.block(0,0,3,3);
Eigen::Matrix3d wRc2 = P2.block(0,0,3,3);
Eigen::Matrix3d wRc3 = P3.block(0,0,3,3);
Eigen::Vector3d wtc1 = P1.block(0,3,3,1);
Eigen::Vector3d wtc2 = P2.block(0,3,3,1);
Eigen::Vector3d wtc3 = P3.block(0,3,3,1);
Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2;
Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3;
Eigen::Vector3d c1tc2 = wRc1.transpose()*(wtc2-wtc1);
Eigen::Vector3d c1tc3 = wRc1.transpose()*(wtc3-wtc1);
Eigen::Matrix<Derived, 3, 3> wRc1 = P1.block(0,0,3,3);
Eigen::Matrix<Derived, 3, 3> wRc2 = P2.block(0,0,3,3);
Eigen::Matrix<Derived, 3, 3> wRc3 = P3.block(0,0,3,3);
Eigen::Matrix<Derived, 3, 1> wtc1 = P1.block(0,3,3,1);
Eigen::Matrix<Derived, 3, 1> wtc2 = P2.block(0,3,3,1);
Eigen::Matrix<Derived, 3, 1> wtc3 = P3.block(0,3,3,1);
Eigen::Matrix<Derived, 3, 3> c1Rc2 = wRc1.transpose()*wRc2;
Eigen::Matrix<Derived, 3, 3> c1Rc3 = wRc1.transpose()*wRc3;
Eigen::Matrix<Derived, 3, 1> c1tc2 = wRc1.transpose()*(wtc2-wtc1);
Eigen::Matrix<Derived, 3, 1> c1tc3 = wRc1.transpose()*(wtc3-wtc1);
c2Pc1.block(0,0,3,3) = c1Rc2.transpose();
c2Pc1.block(0,3,3,1) = -c1Rc2.transpose()*c1tc2;
......@@ -314,7 +326,6 @@ inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixBa
c3Pc1.block(0,3,3,1) = -c1Rc3.transpose()*c1tc3;
}
// Trifocal tensor from Projection matrices (c1->c2 and c1->c3)
for(size_t ii = 0; ii<3; ++ii)
for(size_t jj = 0; jj<3; ++jj)
......@@ -324,7 +335,8 @@ inline bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixBa
return true;
}
inline bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
template<typename Derived>
inline bool TrifocalTensorBase<Derived>::computeTensorFactorized(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
{
clock_t tStart = clock();
......@@ -403,7 +415,8 @@ inline bool TrifocalTensor::computeTensorFactorized(const Eigen::MatrixXd& list1
return true;
}
inline bool TrifocalTensor::computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
template<typename Derived>
inline bool TrifocalTensorBase<Derived>::computeTensorRansac(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
{
clock_t tStart = clock();
......@@ -461,7 +474,8 @@ inline bool TrifocalTensor::computeTensorRansac(const Eigen::MatrixXd& list1, co
return true;
}
inline void TrifocalTensor::computeMatrixA(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& A)
template<typename Derived>
inline void TrifocalTensorBase<Derived>::computeMatrixA(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& A)
{
// DLT algorithm (check summary in
//L. Wang and F. C. Wu, "A factorization algorithm for trifocal tensor estimation,"
......@@ -488,7 +502,8 @@ inline void TrifocalTensor::computeMatrixA(const Eigen::MatrixXd& list1, const E
}
}
inline bool TrifocalTensor::resolveEquationWithSVD(Eigen::MatrixXd& A, Eigen::VectorXd& tensorVector)
template<typename Derived>
inline bool TrifocalTensorBase<Derived>::resolveEquationWithSVD(Eigen::MatrixXd& A, Eigen::VectorXd& tensorVector)
{
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
......@@ -509,8 +524,8 @@ inline bool TrifocalTensor::resolveEquationWithSVD(Eigen::MatrixXd& A, Eigen::Ve
return true;
}
inline void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, Eigen::MatrixXd& B, Eigen::VectorXd& b)
template<typename Derived>
inline void TrifocalTensorBase<Derived>::computeMatrixB(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, Eigen::MatrixXd& B, Eigen::VectorXd& b)
{
B.setZero(4, 2);
b.setZero(4, 1);
......@@ -526,7 +541,8 @@ inline void TrifocalTensor::computeMatrixB(const Eigen::Vector3d& p1, const Eige
}
}
inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , const Eigen::Vector3d& p2, Eigen::Vector3d& p3, const Eigen::Matrix3d& T_norm) {
template<typename Derived>
inline void TrifocalTensorBase<Derived>::transfer(const Eigen::Vector3d& p1 , const Eigen::Vector3d& p2, Eigen::Vector3d& p3, const Eigen::Matrix3d& T_norm) {
Eigen::MatrixXd B;
Eigen::VectorXd b;
......@@ -552,7 +568,8 @@ inline void TrifocalTensor::transfer(const Eigen::Vector3d& p1 , const Eigen::Ve
p3 = T_norm.inverse()*pn3;
}
inline Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
template<typename Derived>
inline Eigen::MatrixXd TrifocalTensorBase<Derived>::tensorProduct(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
{
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(A.rows()*B.rows(),A.cols()*B.cols());
for (int ra=0;ra<A.rows();ra++)
......@@ -565,7 +582,8 @@ inline Eigen::MatrixXd TrifocalTensor::tensorProduct(const Eigen::MatrixXd& A, c
return C;
}
inline void TrifocalTensor::normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3, Eigen::Matrix3d& T_norm)
template<typename Derived>
inline void TrifocalTensorBase<Derived>::normalizeIsotrop(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3, Eigen::MatrixXd& l1, Eigen::MatrixXd& l2, Eigen::MatrixXd& l3, Eigen::Matrix3d& T_norm)
{
Eigen::MatrixXd list(list1.rows()+list2.rows()+list3.rows(),3);
list.block(0,0,list1.rows(),3) = list1;
......@@ -578,6 +596,12 @@ inline void TrifocalTensor::normalizeIsotrop(const Eigen::MatrixXd& list1, const
l3 = listnorm.block(list1.rows()+list2.rows(),0,list3.rows(),3);
}
// Trifocal tensor class. This is done to avoid specifing the base class templatized type (Is Scalar by deault Scalar).
typedef TrifocalTensorBase<Scalar> TrifocalTensor;
typedef std::shared_ptr<TrifocalTensor> TrifocalTensorPtr;
typedef std::shared_ptr<const TrifocalTensor> TrifocalTensorConstPtr;
typedef std::weak_ptr<TrifocalTensor> TrifocalTensorWPtr;
} /* namespace vision_utils */
......
src/test/gtest_trifocaltensor.cpp
+ 5
− 5
View file @ 455bada2
......@@ -67,8 +67,8 @@ std::map<int, cv::Point2f> TestDetectorROI(const cv::Mat& _image, vision_utils::
#ifdef _VU_DEBUG
cv::Mat image_graphics = _image.clone();
cv::drawKeypoints(image_graphics,target_keypoints,image_graphics);
cv::rectangle(image_graphics, roi, cv::Scalar(255.0, 0.0, 255.0), 1, 8, 0);
cv::rectangle(image_graphics, roi_inflated, cv::Scalar(255.0, 255.0, 0.0), 1, 8, 0);
cv::rectangle(image_graphics, roi, cv::vision_utils::Scalar(255.0, 0.0, 255.0), 1, 8, 0);
cv::rectangle(image_graphics, roi_inflated, cv::vision_utils::Scalar(255.0, 255.0, 0.0), 1, 8, 0);
cv::imshow("test",image_graphics);
cv::waitKey(1);
#endif
......@@ -509,11 +509,11 @@ TEST(TrifocalTensor, ComputeTensorReal)
#ifdef _VU_DEBUG
if (err_tmp.norm()<=std::pow(max_error_reprojection,2))
cv::circle(img_tmp,cv::Point(p3est(0),p3est(1)),5,cv::Scalar(0,255,0),-1);
cv::circle(img_tmp,cv::Point(p3est(0),p3est(1)),5,cv::vision_utils::Scalar(0,255,0),-1);
else
{
cv::circle(img_tmp,cv::Point(list3(ii,0),list3(ii,1)),5,cv::Scalar(0,0,255),-1);
cv::circle(img_tmp,cv::Point(p3est(0),p3est(1)),5,cv::Scalar(255,0,0),2);
cv::circle(img_tmp,cv::Point(list3(ii,0),list3(ii,1)),5,cv::vision_utils::Scalar(0,0,255),-1);
cv::circle(img_tmp,cv::Point(p3est(0),p3est(1)),5,cv::vision_utils::Scalar(255,0,0),2);
}
#endif
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment