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

WIP trifocal

parent 86b239f9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/common_class/trifocaltensor.cpp
+ 33
− 33
View file @ 3093cf44
......@@ -64,7 +64,7 @@ std::vector<std::vector<int> > TrifocalTensor::getAllComb(const int& nelements)
return list;
}
void 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, const std::vector<int>& distances, std::vector<int>& candidate_distances)
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, const std::vector<int>& distances, std::vector<int>& candidate_distances)
{
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()
......@@ -96,11 +96,13 @@ void TrifocalTensor::getRandomCandidates(const int& min_points, const Eigen::Mat
l2.row(ii) = list2.row(idx(ii));
l3.row(ii) = list3.row(idx(ii));
}
return idx;
}
double TrifocalTensor::getErrorTrifocal(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
{
double count = 0.0;
// double count = 0.0;
double error = 0.0;
for (int ii=0; ii<list1.rows(); ++ii)
{
......@@ -111,20 +113,16 @@ double TrifocalTensor::getErrorTrifocal(const Eigen::MatrixXd& list1, const Eige
transfer(p1, p2, p3est);
p3est = p3est/p3est(2);
Eigen::Vector3d res = p3est - p3;
error = res.transpose()*res;
double max_reprojection_error_squared = max_reprojection_error_*max_reprojection_error_;
if (error < max_reprojection_error_squared)
count = count+1;
// std::cout << std::sqrt(error) << std::endl;
error += res.norm();
}
return count/list1.rows();
return error;
}
bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::MatrixXd& list2, const Eigen::MatrixXd& list3)
{
clock_t tStart = clock();
double correct_statistics = 0;
double error_best = 0;
float tensor_final[3][3][3];
Eigen::MatrixXd A;
......@@ -136,11 +134,11 @@ bool TrifocalTensor::computeTensor(const Eigen::MatrixXd& list1, const Eigen::Ma
{
fillTensor(tensorVector);
double correct_statistics_iter = getErrorTrifocal(list1, list2, list3);
double error_curr = getErrorTrifocal(list1, list2, list3);
if (correct_statistics_iter>correct_statistics)
if (error_curr>error_best)
{
correct_statistics = correct_statistics_iter;
error_best = error_curr;
memcpy( tensor_final, tensor_, 27*sizeof(int) );
}
}
......@@ -158,7 +156,7 @@ bool TrifocalTensor::computeTensor(const std::vector<int>& distances, const Eige
{
//=======================================
// Sort by interdistances
//// Sort by interdistances
// std::cout << "size list1: " << list1.rows() << std::endl;
//
// std::vector<int> dist;
......@@ -180,38 +178,41 @@ bool TrifocalTensor::computeTensor(const std::vector<int>& distances, const Eige
// for(int ii=0; ii<dist.size(); ++ii)
// std::cout << "Sorted: " << dist[idx[dist.size()-1-ii]] << " idx: " << idx[dist.size()-1-ii] << std::endl;
// std::cout << std::endl;
//
// std::cout << "idx ordered: " << idx[dist.size()-1] << "," << idx[dist.size()-2] << "," << idx[dist.size()-3] << "," << idx[dist.size()-4] << "," << idx[dist.size()-5] << "," << idx[dist.size()-6] << "," << idx[dist.size()-7] << "," << idx[dist.size()-8] << std::endl;
//================
// //================
clock_t tStart = clock();
int min_points = 8;
// int min_points = list1.rows();
if (list1.rows()<min_points || list2.rows()<min_points || list3.rows()<min_points)
return false;
double correct_statistics = 0;
double error_best = 1e3;
int iter = 0;
float tensor_final[3][3][3];
std::vector<int> final_distance(min_points);
Eigen::VectorXd idxs_best;
// Non random
// max_iterations_ = list1.rows()-min_points+1;
while (correct_statistics<percentage_of_correct_reprojected_points_ && iter<max_iterations_)
// while (error_best<percentage_of_correct_reprojected_points_ && iter<max_iterations_)
// while (error_best>0.5 && iter<max_iterations_)
while (iter<max_iterations_)
{
iter=iter+1;
// std::cout << "iter: " << iter << std::endl;
std::vector<int> candidate_distance;
Eigen::MatrixXd l1(min_points,3), l2(min_points,3), l3(min_points,3);
getRandomCandidates(min_points, list1, list2, list3, l1, l2, l3, distances, candidate_distance);
Eigen::VectorXd idxs = getRandomCandidates(min_points, list1, list2, list3, l1, l2, l3, distances, candidate_distance);
// // Use sorted list
// for (int ii=0;ii<min_points;++ii)
// {
// l1.row(ii) = list1.row(idx[idx.size()-1-ii]);
......@@ -219,12 +220,8 @@ bool TrifocalTensor::computeTensor(const std::vector<int>& distances, const Eige
// l3.row(ii) = list3.row(idx[idx.size()-1-ii]);
// }
// l1 = list1;
// l2 = list2;
// l3 = list3;
// Non random
//// Non random
// std::vector<int>::const_iterator first = distances.begin() + iter;
// std::vector<int>::const_iterator last = distances.begin() + iter + min_points;
// std::vector<int> candidate_distance(first, last);
......@@ -237,11 +234,11 @@ bool TrifocalTensor::computeTensor(const std::vector<int>& distances, const Eige
// l1 = list1.block(iter,0,min_points,3);
// l2 = list2.block(iter,0,min_points,3);
// l3 = list3.block(iter,0,min_points,3);
//
// iter=iter+1;
// TMP
// // TMP
//
// double sum = std::accumulate(candidate_distance.begin(), candidate_distance.end(), 0.0);
// double mean = sum / candidate_distance.size();
......@@ -261,12 +258,12 @@ bool TrifocalTensor::computeTensor(const std::vector<int>& distances, const Eige
{
fillTensor(tensorVector);
double correct_statistics_iter = getErrorTrifocal(list1, list2, list3);
double error_curr = getErrorTrifocal(list1, list2, list3)/list1.rows();
if (correct_statistics_iter>correct_statistics)
if (error_curr<error_best)
{
final_distance = candidate_distance;
correct_statistics = correct_statistics_iter;
error_best = error_curr;
memcpy( tensor_final, tensor_, 27*sizeof(int) );
}
}
......@@ -275,9 +272,12 @@ bool TrifocalTensor::computeTensor(const std::vector<int>& distances, const Eige
}
memcpy( tensor_, tensor_final, 27*sizeof(int) );
std::cout << " error of winner" << error_best << std::endl;
std::cout << "=============" << std::endl;
for (auto dst : final_distance)
std::cout << dst << std::endl;
std::cout << "best point idxs: " << idxs_best.transpose() << std::endl;
// for (auto dst : final_distance)
// std::cout << dst << std::endl;
std::cout << "=============" << std::endl;
// TMP
......
src/common_class/trifocaltensor.h
+ 1
− 1
View file @ 3093cf44
......@@ -91,7 +91,7 @@ private:
/**
* \brief Get random candidates
*/
void 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, const std::vector<int>& distances, std::vector<int>& candidate_distances);
Eigen::VectorXd 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, const std::vector<int>& distances, std::vector<int>& candidate_distances);
};
......
src/test/gtest_trifocaltensor.cpp
+ 3
− 3
View file @ 3093cf44
......@@ -391,7 +391,7 @@ TEST(TrifocalTensor, ComputeTensorReal)
DMatchVector matches12;
mat_ptr->match(desc1,desc2,matches12);
//ASSERT_EQ(matches12.size(),196);
ASSERT_EQ(matches12.size(),200);
// ASSERT_EQ(matches12.size(),200);
// filter
for (int ii = 0; ii < matches12.size(); ++ii )
......@@ -473,7 +473,7 @@ TEST(TrifocalTensor, ComputeTensorReal)
Eigen::MatrixXd list3 = vision_utils::KeyPointVecToEigenMat(kpts_good_in3);
// Tensor parameters
int max_iterations = 5000;
int max_iterations = 50000;
double max_error_reprojection = 1.0;
double percentage_of_correct_reprojected_points = 1.0;
......@@ -516,7 +516,7 @@ TEST(TrifocalTensor, ComputeTensorReal)
//#ifdef _VU_DEBUG
cv::imshow("test2",img_tmp);
cv::waitKey(1000);
cv::waitKey(100000);
//#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