C++ MatrixXf::block方法代码示例

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;
}