void CutMask::protectCore(MatrixXf & overlap, BlockType type)
{
switch(type)
{
case VERTICAL:
overlap.block(0, BandSize, BlockSize, BlockSize - BandSize) = MatrixXf::Constant(BlockSize, BlockSize - BandSize, FLT_MAX);
break;
case V_BOTHSIDES:
{
int width = BlockSize - (2 * BandSize);
overlap.block(0, BandSize, BlockSize, width) = MatrixXf::Constant(BlockSize, width, FLT_MAX);
}break;
case L_SHAPED:
{
int size = BlockSize - BandSize;
overlap.block(BandSize, BandSize, size, size) = MatrixXf::Constant(size, size, FLT_MAX);
}break;
case N_SHAPED:
{
int width = BlockSize - (2 * BandSize);
int height = BlockSize - BandSize;
overlap.block(BandSize, BandSize, height, width) = MatrixXf::Constant(height, width, FLT_MAX);
}break;
case NONE_BLOCK:
case HORIZONTAL:
break;
}
}
void call_ref()
{
VectorXcf ca = VectorXcf::Random(10);
VectorXf a = VectorXf::Random(10);
RowVectorXf b = RowVectorXf::Random(10);
MatrixXf A = MatrixXf::Random(10,10);
RowVector3f c = RowVector3f::Random();
const VectorXf& ac(a);
VectorBlock<VectorXf> ab(a,0,3);
const VectorBlock<VectorXf> abc(a,0,3);
VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
// call_ref_1(ac,a<c); // does not compile because ac is const
VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
// call_ref_1(A.row(3),A.row(3)); // does not compile because innerstride!=1
VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
// call_ref_1(a+a, a+a); // does not compile for obvious reason
MatrixXf tmp = A*A.col(1);
VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
// call_ref_5(A.transpose(),A.transpose()); // does not compile because storage order does not match
VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0); // storage order do not match, but this is a degenerate case that should work
VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
tmp = A+A;
VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1); // evaluated into a temp because the storage orders do not match
VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
}
// normalizeMatch respect to "In defense of eight point algorithm"
void normalizeMatch(MatrixXf &mat, Matrix3f &T1, Matrix3f &T2) {
MatrixXf pts1 = mat.leftCols<3>();
MatrixXf pts2 = mat.block(0, 3, mat.rows(), 3);
normalizePts(pts1, T1);
normalizePts(pts2, T2);
mat.leftCols<3>() = pts1;
mat.block(0, 3, mat.rows(), 3) = pts2;
}
void toHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 2) {
temp.resize(mat.rows(), 3);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
mat = temp;
} else if (mat.cols() == 4) {
temp.resize(mat.rows(), 6);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
temp.block(0, 3, mat.rows(), 2) = temp.block(0, 2, mat.rows(), 2);
temp.col(5).setConstant(1);
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
bool singleModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int maxInlier = -1;
MatrixXf bestResidue;
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount ++;
RandomSampling(psize, dataSize, sample);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
Matrix3f local_H;
MatrixXf local_A;
fitHomography(pts1, pts2, local_H, local_A);
MatrixXf residue;
computeHomographyResidue(x1, x2, local_H, residue);
int inlierCount = (residue.array() < THRESHOLD).count();
if (inlierCount > maxInlier) {
maxInlier = inlierCount;
bestResidue = residue;
}
}
inlier.resize(maxInlier, data.cols());
int transferCounter = 0;
for (int i = 0; i < dataSize; i++) {
if (bestResidue(i) < THRESHOLD) {
inlier.row(transferCounter) = data.row(i);
transferCounter++;
}
}
if (transferCounter != maxInlier) {
cout << "RANSAC result size does not match!!!!" << endl;
return false;
}
return true;
}
void compressFeature( string filename, std::vector< std::vector<float> > &models, const int dim, bool ascii ){
PCA pca;
pca.read( filename.c_str(), ascii );
VectorXf variance = pca.getVariance();
MatrixXf tmpMat = pca.getAxis();
MatrixXf tmpMat2 = tmpMat.block(0,0,tmpMat.rows(),dim);
const int num = (int)models.size();
for( int i=0; i<num; i++ ){
Map<VectorXf> vec( &(models[i][0]), models[i].size() );
//vec = tmpMat2.transpose() * vec;
VectorXf tmpvec = tmpMat2.transpose() * vec;
models[i].resize( dim );
if( WHITENING ){
for( int t=0; t<dim; t++ )
models[i][t] = tmpvec[t] / sqrt( variance( t ) );
}
else{
for( int t=0; t<dim; t++ )
models[i][t] = tmpvec[t];
}
}
}
int main(int argc, char** argv)
{
// data
vector<completeInformation> compl_info, selected_info;
vector<vecPairsList> extractedLines;
// read robot's info
parseRobotInfo("/home/ubisum/fuerte_workspace/thesis_pack/src/files/robotInfo.txt", compl_info);
// select relevant poses
selected_info = selectFrames(compl_info);
// compute lines for each pose
for(int i = 0; i<(int)selected_info.size(); i++)
{
Vector2fVector vector;
for(int j = 0; j<(int)selected_info[i].points.size(); j++)
{
// extract a coordinate of a scan's point
coordinate coord = selected_info[i].points[j];
// fill a vector
Vector2f temp_vec;
temp_vec << coord.first, coord.second;
// store vector
vector.push_back(temp_vec);
}
// compute and store lines for current scan
extractedLines.push_back(computeLines(vector));
}
Vector4f a(15,12,24,20);
obtainNewExtremes(a,Vector4f(15,16,19,22));
//cout << "Projection " << endl << projectPoint(Vector4f(15,12,24,20), Vector2f(10,14)) << endl;
cout << "Projection " << endl << projectByEquation(Vector3f(2,-1,0), Vector2f(2,3)) << endl;
// vector<vecPairsList> reducedVector(extractedLines.begin()+1, extractedLines.begin()+2);
int init = 0;
int end = 1;
vector<vecPairsList> reducedVector;
for(int k = init; k<= end; k++)
reducedVector.push_back(extractedLines[k]);
//MatrixXf m = mergeLines(createVector());
MatrixXf m = mergeLines(reducedVector);
//MatrixXf m = mergeLines(extractedLines);
cout << "Size m: " << m.rows() << " " << m.cols() << endl;
printLinesByExtremes(m.block(6,0,4,m.cols()), MatrixXf::Identity(3,3), "merged_lines.txt");
cout << endl << "Proiezione punto" << endl;
cout << projectByEquation(Vector3f(2, -1, 0), Vector2f(2,3)) << endl << endl;
Vector4f line1(2,3,2,1);
Vector4f line2(2,1,5,1);
MatrixXf two_lines(4,2);
two_lines.block(0,0,4,1) = line1;
two_lines.block(0,1,4,1) = line2;
float angle = 45*M_PI/180;
MatrixXf prova_t(3,3);
prova_t << cos(angle), -sin(angle), 5, sin(angle), cos(angle), 2, 0, 0, 1;
// prova_t << 1,0,5,0,1,0,0,0,1;
// MatrixXf prova_t = MatrixXf::Identity(3,3);
MatrixXf transf_vectors(4,2);
transf_vectors.block(0,0,2,2) = transformVectors(two_lines.block(0,0,2,2), prova_t);
transf_vectors.block(2,0,2,2) = transformVectors(two_lines.block(2,0,2,2), prova_t);
cout << "transf_vectors" << endl << transf_vectors << endl;
remove("prova_transf.txt");
FILE* prova_transf = fopen("prova_transf.txt", "a");
stringstream ss_prova;
for(int w = 0; w<transf_vectors.cols(); w++)
{
Vector4f column = transf_vectors.block(0,w,4,1);
ss_prova << column(0) << "\t" << column(1) << "\n" <<
column(2) << "\t" << column(3) << "\n\n";
fputs(ss_prova.str().c_str(), prova_transf);
}
Vector2f polar1 = polarRepresentation(line1.block(0,0,2,1), line1.block(2,0,2,1));
//Vector2f polar2 = polarRepresentation(transf_vectors.block(0,0,2,1), transf_vectors.block(2,0,2,1));
Vector2f polar2 = new_transformRT(transf_vectors.block(0,0,4,1));
cout << "differenza theta " << 180*fabs(polar1(1)-polar2(1))/M_PI << endl;
}
bool multiModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
int blockSize = 10;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int h = 0;
MatrixXf Hs(M, 9);
MatrixXf inx(M, psize);
MatrixXf res(dataSize, M);
MatrixXi resIndex(dataSize, M);
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount++;
if (m < blockSize)
RandomSampling(psize, dataSize, sample);
else
WeightedSampling(psize, dataSize, resIndex, sample, h);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
for (int i = 0; i < psize; i++)
inx(m, i) = sample[i];
Matrix3f temp_H;
MatrixXf temp_A, localResidue;
fitHomography(pts1, pts2, temp_H, temp_A);
computeHomographyResidue(x1, x2, temp_H, localResidue);
Hs.row(m) = unrollMatrix3f(temp_H);
res.col(m) = localResidue;
if (m >= (blockSize-1) && (m+1)%blockSize == 0) {
h = round(0.1f*m);
sortResidueForIndex(res, (m/blockSize)*blockSize, ((m+1)/blockSize)*blockSize, resIndex);
}
}
VectorXf bestModel(M);
bestModel.setZero();
int bestIndex = 0;
int bestCount = -1;
for (int i = 0; i < M; i++) {
for (int j = 0; j < dataSize; j++)
if (res(j, i) < THRESHOLD)
bestModel(i) += 1;
if (bestModel(i) > bestCount) {
bestIndex = i;
bestCount = bestModel(i);
}
}
VectorXf bestModelRes = res.col(bestIndex);
int inlierCount = (bestModelRes.array() < THRESHOLD).count();
inlier.resize(inlierCount, data.cols());
int runningIdx = 0;
for (int i = 0; i < dataSize; i++)
if (bestModelRes(i) < THRESHOLD) {
inlier.row(runningIdx) = data.row(i);
runningIdx ++;
}
return true;
}