int computeNoInliersFundamental(mat &data, mat &f, ublas::vector<bool> &inlier_mask, double &dist_std)
{
double T_DIST = 0.5;//dist>0.1 pixels = outlier might want to make it sqrt(5.99)*sigma
int num_inlier = 0;
double m_dist=0;
dist_std=0;
vec dist(data.size1());
inlier_mask.resize(data.size1());
for (int i=0;i<data.size1();i++)
inlier_mask(i)=false;
for (int i=0;i<data.size1();i++)
{
vec x1(3),x2(3);
x1(0)=data(i,0);x1(1)=data(i,1);x1(2)=1;
x2(0)=data(i,2);x2(1)=data(i,3);x2(2)=1;
dist(i) = fabs(inner_prod(prod (x2, f),x1));//x2 F x1
if (dist(i)<T_DIST)
{
num_inlier++;
inlier_mask(i)=true;
m_dist+=dist(i);
}
}
m_dist = m_dist/ num_inlier;
for (int i=0;i<data.size1();i++)
if (inlier_mask(i))
dist_std += pow(dist(i)-m_dist,2);
dist_std = dist_std/(num_inlier-1);
return num_inlier;
}
bool readVectorFromBinaryFile(const std::string & filename, boost::numeric::ublas::vector<TYPE> & vec)
{
std::ifstream file(filename.c_str(), std::ios_base::binary);
if (!file) return false;
unsigned int size;
file.read((char*)&size, sizeof(unsigned int));
vec.resize(size);
file.read((char*)&vec[0], sizeof(TYPE)*size);
return true;
}
bool readVectorFromFile(const std::string & filename, boost::numeric::ublas::vector<TYPE> & vec)
{
std::ifstream file(filename.c_str());
if (!file) return false;
unsigned int size;
file >> size;
vec.resize(size);
for (unsigned int i = 0; i < size; ++i)
{
TYPE element;
file >> element;
vec[i] = element;
}
return true;
}
bool read_vector(FILE* file, boost::numeric::ublas::vector<T>& v) {
unsigned magic, rowCount, columnCount;
if (fread(&magic, sizeof(unsigned), 1, file) != 1)
return false;
if (!check_magic<T>(magic))
return false;
if (fread(&rowCount, sizeof(unsigned), 1, file) != 1)
return false;
if (fread(&columnCount, sizeof(unsigned), 1, file) != 1)
return false;
const unsigned count = rowCount * columnCount;
if (count != v.size())
v.resize(count, false);
T* buffer = new T[count];
if (fread(buffer, sizeof(T), count, file) != count)
return false;
std::copy(buffer, buffer + count, v.begin());
return true;
}
