C++ Matrix3d::row方法代码示例

Eigen::Matrix3d make_inv_matr(
    SuperCell& SCell
){
    Eigen::Matrix3d invmat;
    invmat.row(0) = ( SCell.T.row(1) ).cross( (SCell.T.row(2) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
    invmat.row(1) = ( SCell.T.row(2) ).cross( (SCell.T.row(0) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) ); 
    invmat.row(2) = ( SCell.T.row(0) ).cross( (SCell.T.row(1) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
    return invmat;
}

  void CETranslateWidget::checkSelection()
  {
    // User closed the dialog:
    if (this->isHidden()) {
      m_selectionTimer.stop();
      return;
    }

    // Improper initialization
    if (m_gl == NULL)
      return setError(tr("No GLWidget?"));

    QList<Primitive*> atoms = m_gl->selectedPrimitives().subList
      (Primitive::AtomType);

    // No selection
    if (!atoms.size())
      return setError(tr("Please select one or more atoms."));

    clearError();

    // Calculate centroid of selection
    m_vector = Eigen::Vector3d::Zero();
    for (QList<Primitive*>::const_iterator
           it = atoms.constBegin(),
           it_end = atoms.constEnd();
         it != it_end; ++it) {
      m_vector += (*qobject_cast<Atom* const>(*it)->pos());
    }
    m_vector /= static_cast<double>(atoms.size());

    switch (static_cast<TranslateMode>
            (ui.combo_translateMode->currentIndex())) {
    default:
    case Avogadro::CETranslateWidget::TM_VECTOR:
      // Shouldn't happen, but just in case...
      m_selectionTimer.stop();
      this->enableVectorEditor();
      break;
    case Avogadro::CETranslateWidget::TM_ATOM_TO_ORIGIN:
      m_vector = -m_vector;
      break;
    case Avogadro::CETranslateWidget::TM_ATOM_TO_CELL_CENTER:
      // Calculate center of unit cell
      const Eigen::Matrix3d cellRowMatrix = m_ext->currentCellMatrix();
      const Eigen::Vector3d center = 0.5 *
          (cellRowMatrix.row(0) + cellRowMatrix.row(1) + cellRowMatrix.row(2));

      // Calculate necessary translation
      m_vector = center - m_vector;
      break;
    }

    updateGui();
  }

void Reconstruction3D::RQdecomposition(Eigen::MatrixXd A, Eigen::Matrix3d &R, Eigen::Matrix3d &Q)
{
	float c, s;
	Eigen::Matrix3d Qx, Qy, Qz;
	Eigen::Matrix3d M;
	M << A(0, 0), A(0, 1), A(0, 2),
		A(1, 0), A(1, 1), A(1, 2),
		A(2, 0), A(2, 1), A(2, 2);

	R = M;
	Reconstruction3D::solveCS(c, s, R(2, 1), R(2, 2));
	Qx << 1, 0, 0,
		0, c, -s,
		0, s, c;
	R *= Qx;

	Reconstruction3D::solveCS(c, s, R(2, 0), -R(2, 2));
	Qy << c, 0, s,
		0, 1, 0,
		-s, 0, c;
	R *= Qy;

	Reconstruction3D::solveCS(c, s, R(1, 0), R(1, 1));
	Qz << c, -s, 0,
		s, c, 0,
		0, 0, 1;
	R *= Qz;

	if (std::abs(R(1, 0)) > 0.00005 || std::abs(R(2, 0)) > 0.00005 || std::abs(R(2, 1)) > 0.00005)
		std::cerr << "PROBLEM WITH RQdecomposition" << std::endl;;

	//    if(R(1,0)!= 0) R(1,0) = 0;
	//    if(R(2,0)!= 0) R(2,0) = 0;
	//    if(R(2,1)!= 0) R(2,1) = 0;


	Q = Qz.transpose()  * Qy.transpose() * Qx.transpose();

	if (R(0, 0) < 0)
	{
		R.col(0) *= -1;
		Q.row(0) *= -1;
	}
	if (R(1, 1) < 0)
	{
		R.col(1) *= -1;
		Q.row(1) *= -1;
	}
	if (R(2, 2) < 0)
	{
		R.col(2) *= -1;
		Q.row(2) *= -1;
	}
}

void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions)
{
    // compute mean
    Eigen::Vector3d cm;
    cm.setZero();
    for (size_t i = 0; i < positions.size(); i++) {
        cm += positions[i];
    }
    cm /= (double)positions.size();
    
    // adjust for mean and compute covariance matrix
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (size_t i = 0; i < positions.size(); i++) {
        Eigen::Vector3d pAdg = positions[i] - cm;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)positions.size();
    
    // compute eigenvectors for covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
    
    // set axes
    eigenVectors.transpose();
    xAxis = eigenVectors.row(0);
    yAxis = eigenVectors.row(1);
    zAxis = eigenVectors.row(2);
    
    // project min and max points on each principal axis
    double min1 = INF, max1 = -INF;
    double min2 = INF, max2 = -INF;
    double min3 = INF, max3 = -INF;
    double d = 0.0;
    for (size_t i = 0; i < positions.size(); i++) {
        d = xAxis.dot(positions[i]);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = yAxis.dot(positions[i]);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = zAxis.dot(positions[i]);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // set center and halflengths
    center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2;
    halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2;
}

  void CEViewOptionsWidget::updateMillerPlane()
  {
    // View into a Miller plane
    Camera *camera = m_glWidget->camera();
    Eigen::Transform3d modelView;
    modelView.setIdentity();

    // OK, so we want to rotate to look along the normal at the plane
    // So we convert <h k l> into a Cartesian normal
    Eigen::Matrix3d cellMatrix = m_ext->unconvertLength(m_ext->currentCellMatrix()).transpose();
    // Get miller indices:
    const Eigen::Vector3d millerIndices
      (static_cast<double>(ui.spin_mi_h->value()),
       static_cast<double>(ui.spin_mi_k->value()),
       static_cast<double>(ui.spin_mi_l->value()));
    // Check to see if we have 0,0,0
    //  in which case, we do nothing
    if (millerIndices.squaredNorm() < 0.5)
      return;

    const Eigen::Vector3d normalVector ((cellMatrix * millerIndices).normalized());

    Eigen::Matrix3d rotation;
    rotation.row(2) = normalVector;
    rotation.row(0) = rotation.row(2).unitOrthogonal();
    rotation.row(1) = rotation.row(2).cross(rotation.row(0));

    // Translate camera to the center of the cell
    const Eigen::Vector3d cellDiagonal =
        cellMatrix.col(0) * m_glWidget->aCells() +
        cellMatrix.col(1) * m_glWidget->bCells() +
        cellMatrix.col(2) * m_glWidget->cCells();

    modelView.translate(-cellDiagonal*0.5);

    // Prerotate the camera to look down the specified normal
    modelView.prerotate(rotation);


    // Pretranslate in the negative Z direction
    modelView.pretranslate(Eigen::Vector3d(0.0, 0.0,
                                           -1.5 * cellDiagonal.norm()));

    camera->setModelview(modelView);

    // Call for a redraw
    m_glWidget->update();
  }

    void EKF::predict(Eigen::Matrix3d Hom)
    {
        // homogeneous coordinates
        Eigen::Vector3d x_tilde;
        x_tilde  << this->x(0), this->x(1), 1.0;

        // intermediary values
        double gx   = Hom.row(0) * x_tilde;
        double gy   = Hom.row(1) * x_tilde;
        double h    = Hom.row(2) * x_tilde;

        // update transition matrix
        Eigen::Matrix2d F;
        F << 
            (Hom(0, 0) * h - Hom(2, 0) * gx) / (h*h),   (Hom(0, 1) * h - Hom(2, 1) * gx) / (h*h),
            (Hom(1, 0) * h - Hom(2, 0) * gy) / (h*h),   (Hom(1, 1) * h - Hom(2, 1) * gy) / (h*h);

        // propogate
        this->x = Eigen::Vector2d(gx / h, gy / h);
        this->P = F * this->P * F.transpose() + this->Q;
        
    }

void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
    type = "Oriented";
    orientedPoints.clear();
    
    // compute mean
    Eigen::Vector3d center;
    center.setZero();
    for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
        center += v->position;
    }
    center /= (double)vertices.size();
    
    // adjust for mean and compute covariance
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        Eigen::Vector3d pAdg = v->position - center;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)vertices.size();

    // compute eigenvectors for the covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();

    // project min and max points on each principal axis
    double min1 = INFINITY, max1 = -INFINITY;
    double min2 = INFINITY, max2 = -INFINITY;
    double min3 = INFINITY, max3 = -INFINITY;
    double d = 0.0;
    eigenVectors.transpose();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        d = eigenVectors.row(0).dot(v->position);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = eigenVectors.row(1).dot(v->position);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = eigenVectors.row(2).dot(v->position);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // add points to vector
    orientedPoints.push_back(eigenVectors.row(0) * min1);
    orientedPoints.push_back(eigenVectors.row(0) * max1);
    orientedPoints.push_back(eigenVectors.row(1) * min2);
    orientedPoints.push_back(eigenVectors.row(1) * max2);
    orientedPoints.push_back(eigenVectors.row(2) * min3);
    orientedPoints.push_back(eigenVectors.row(2) * max3);
}

void
stereo_rectify(const CameraIntrinsicsParameters& left_params,
               const CameraIntrinsicsParameters& right_params,
               const Eigen::Quaterniond& rotation_quat,
               const Eigen::Vector3d& translation,
               Eigen::Matrix3d* left_rotation,
               Eigen::Matrix3d* right_rotation,
               CameraIntrinsicsParameters* rectified_params)
{
    // Get two cameras to same orientation, minimally rotating each.
    Eigen::AngleAxisd rect_rot(rotation_quat);
    rect_rot.angle() *= -0.5;

    Eigen::Vector3d rect_trans = rect_rot * translation;
    // Bring translation to alignment with (1, 0, 0).
    Eigen::Matrix3d rot;
    rot.row(0) = -rect_trans;
    rot.row(0).normalize();
    rot.row(1) = Eigen::Vector3d(0, 0, 1).cross(rot.row(0));
    rot.row(2) = rot.row(0).cross(rot.row(1));

    //std::cerr << "rot =\n" << rot << std::endl << std::endl;

    (*left_rotation) = rot * rect_rot.inverse();
    (*right_rotation) = rot * rect_rot;

    // For now just use left camera's intrinsic parameters
    *rectified_params = left_params;

    // Just to be explicit
    rectified_params->k1 = 0;
    rectified_params->k2 = 0;
    rectified_params->k3 = 0;
    rectified_params->p1 = 0;
    rectified_params->p2 = 0;
}

int P3P::computePoses(const Eigen::Matrix3d & feature_vectors, const Eigen::Matrix3d & world_points,
                      Eigen::Matrix<Eigen::Matrix<double, 3, 4>, 4, 1> & solutions)
{
  // Extraction of world points
  Eigen::Vector3d P1 = world_points.col(0);
  Eigen::Vector3d P2 = world_points.col(1);
  Eigen::Vector3d P3 = world_points.col(2);

  // Verification that world points are not colinear
  Eigen::Vector3d temp1 = P2 - P1;
  Eigen::Vector3d temp2 = P3 - P1;

  if (temp1.cross(temp2).norm() == 0)
  {
    return -1;
  }

  // Extraction of feature vectors
  Eigen::Vector3d f1 = feature_vectors.col(0);
  Eigen::Vector3d f2 = feature_vectors.col(1);
  Eigen::Vector3d f3 = feature_vectors.col(2);

  // Creation of intermediate camera frame
  Eigen::Vector3d e1 = f1;
  Eigen::Vector3d e3 = f1.cross(f2);
  e3 = e3 / e3.norm();
  Eigen::Vector3d e2 = e3.cross(e1);

  Eigen::Matrix3d T;
  T.row(0) = e1.transpose();
  T.row(1) = e2.transpose();
  T.row(2) = e3.transpose();

  f3 = T * f3;

  // Reinforce that f3(2,0) > 0 for having theta in [0;pi]
  if (f3(2, 0) > 0)
  {
    f1 = feature_vectors.col(1);
    f2 = feature_vectors.col(0);
    f3 = feature_vectors.col(2);

    e1 = f1;
    e3 = f1.cross(f2);
    e3 = e3 / e3.norm();
    e2 = e3.cross(e1);

    T.row(0) = e1.transpose();
    T.row(1) = e2.transpose();
    T.row(2) = e3.transpose();

    f3 = T * f3;

    P1 = world_points.col(1);
    P2 = world_points.col(0);
    P3 = world_points.col(2);
  }

  // Creation of intermediate world frame
  Eigen::Vector3d n1 = P2 - P1;
  n1 = n1 / n1.norm();
  Eigen::Vector3d n3 = n1.cross(P3 - P1);
  n3 = n3 / n3.norm();
  Eigen::Vector3d n2 = n3.cross(n1);

  Eigen::Matrix3d N;
  N.row(0) = n1.transpose();
  N.row(1) = n2.transpose();
  N.row(2) = n3.transpose();

  // Extraction of known parameters
  P3 = N * (P3 - P1);

  double d_12 = (P2 - P1).norm();
  double f_1 = f3(0, 0) / f3(2, 0);
  double f_2 = f3(1, 0) / f3(2, 0);
  double p_1 = P3(0, 0);
  double p_2 = P3(1, 0);

  double cos_beta = f1.dot(f2);
  double b = 1 / (1 - pow(cos_beta, 2)) - 1;

  if (cos_beta < 0)
  {
    b = -sqrt(b);
  }
  else
  {
    b = sqrt(b);
  }

  // Definition of temporary variables for avoiding multiple computation
  double f_1_pw2 = pow(f_1, 2);
  double f_2_pw2 = pow(f_2, 2);
  double p_1_pw2 = pow(p_1, 2);
  double p_1_pw3 = p_1_pw2 * p_1;
  double p_1_pw4 = p_1_pw3 * p_1;
  double p_2_pw2 = pow(p_2, 2);
  double p_2_pw3 = p_2_pw2 * p_2;
  double p_2_pw4 = p_2_pw3 * p_2;
//.........这里部分代码省略.........

opengv::translation_t
opengv::absolute_pose::p2p(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1)
{
  Eigen::Vector3d e1 = adapter.getBearingVector(index0);
  Eigen::Vector3d e3 = adapter.getBearingVector(index1);
  e3 = e1.cross(e3);
  e3 = e3/e3.norm();
  Eigen::Vector3d e2 = e3.cross(e1);

  rotation_t T;
  T.row(0) = e1.transpose();
  T.row(1) = e2.transpose();
  T.row(2) = e3.transpose();

  Eigen::Vector3d n1 = adapter.getPoint(index1) - adapter.getPoint(index0);
  n1 = n1/n1.norm();
  Eigen::Vector3d n3;
  if( (fabs(n1[0]) > fabs(n1[1])) && (fabs(n1[0]) > fabs(n1[2])) )
  {
    n3[1] = 1.0;
    n3[2] = 0.0;
    n3[0] = -n1[1]/n1[0];
  }
  else
  {
    if( (fabs(n1[1]) > fabs(n1[0])) && (fabs(n1[1]) > fabs(n1[2])) )
    {
      n3[2] = 1.0;
      n3[0] = 0.0;
      n3[1] = -n1[2]/n1[1];
    }
    else
    {
      n3[0] = 1.0;
      n3[1] = 0.0;
      n3[2] = -n1[0]/n1[2];
    }
  }
  n3 = n3 / n3.norm();
  Eigen::Vector3d n2 = n3.cross(n1);

  rotation_t N;
  N.row(0) = n1.transpose();
  N.row(1) = n2.transpose();
  N.row(2) = n3.transpose();

  Eigen::Matrix3d Q = T * adapter.getR().transpose() * N.transpose();
  Eigen::Vector3d temp1 = adapter.getPoint(index1) - adapter.getPoint(index0);
  double d_12 = temp1.norm();

  Eigen::Vector3d temp2 = adapter.getBearingVector(index1);
  double cos_beta = e1.dot(temp2);
  double b = 1/( 1 - pow( cos_beta, 2 ) ) - 1;

  if( cos_beta < 0 )
    b = -sqrt(b);
  else
    b = sqrt(b);

  double temp3 = d_12 * ( Q(1,0) * b - Q(0,0) );

  translation_t solution = -temp3 * Q.row(0).transpose();
  solution = adapter.getPoint(index0) + N.transpose()*solution;

  if(
    solution(0,0) != solution(0,0) ||
    solution(1,0) != solution(1,0) ||
    solution(2,0) != solution(2,0) )
    solution = Eigen::Vector3d::Zero();

  return solution;
}

//.........这里部分代码省略.........
				{
					redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
				}
			}
		}
		if (height % 2 != 0)
		{
			for (int i = 0; i < width - 1; i+=2)
			{
				int index1 = ((height - 1) * width + i)*ordlen;
				int index2 = ((height - 1) * width + i + 1)*ordlen;
				int offset = ((height2 - 1) * width2 + i/2)*ordlen;
				for (int k = 0; k < basisTerm; k++)
				{
					redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
				}
			}
		}
		if (height % 2 != 0 && width % 2 != 0)
		{
			int index1 = (height*width - 1)*ordlen;
			int offset = (height2*width2 - 1)*ordlen;
			for (int k = 0; k < basisTerm; k++)
			{
				redCoefficients.calcMipMapping(level, offset + k, index1 + k);
				greenCoefficients.calcMipMapping(level, offset + k, index1 + k);
				blueCoefficients.calcMipMapping(level, offset + k, index1 + k);
			}
		}
		mipMapSize[level] = QSize(width2, height2);
	}

	//Compute normals.
	if (cb != NULL) (*cb)(75 , "Normals generation...");
	Eigen::Vector3d l0(sin(M_PI/4)*cos(M_PI/6), sin(M_PI/4)*sin(M_PI/6), cos(M_PI/4));
	Eigen::Vector3d l1(sin(M_PI/4)*cos(5*M_PI / 6), sin(M_PI/4)*sin(5*M_PI / 6), cos(M_PI/4));
	Eigen::Vector3d l2(sin(M_PI/4)*cos(3*M_PI / 2), sin(M_PI/4)*sin(3*M_PI / 2), cos(M_PI/4));
    float hweights0[16], hweights1[16], hweights2[16];
	getHSH(M_PI / 4, M_PI / 6, hweights0, ordlen);
	getHSH(M_PI / 4, 5*M_PI / 6, hweights1, ordlen);
	getHSH(M_PI / 4, 3*M_PI / 2, hweights2, ordlen);
	
	
	Eigen::Matrix3d L;
	L.setIdentity();
	L.row(0) = l0;
	L.row(1) = l1;
	L.row(2) = l2;
	Eigen::Matrix3d LInverse = L.inverse();
	
	for (int level = 0; level < MIP_MAPPING_LEVELS; level++)
	{
		const float* rPtr = redCoefficients.getLevel(level);
		const float* gPtr = greenCoefficients.getLevel(level);
		const float* bPtr = blueCoefficients.getLevel(level);
		vcg::Point3f* temp = new vcg::Point3f[mipMapSize[level].width()*mipMapSize[level].height()];
		if (cb != NULL) (*cb)(75 + level*6, "Normal generation...");

		#pragma omp parallel for
		for (int y = 0; y < mipMapSize[level].height(); y++)
		{
			for (int x = 0; x < mipMapSize[level].width(); x++)
			{
				int offset= y * mipMapSize[level].width() + x;
				Eigen::Vector3d f(0, 0, 0);
				for (int k = 0; k < ordlen; k++)
				{
					f(0) += rPtr[offset*ordlen + k] * hweights0[k];
					f(1) += rPtr[offset*ordlen + k] * hweights1[k];
					f(2) += rPtr[offset*ordlen + k] * hweights2[k];
				}
				for (int k = 0; k < ordlen; k++)
				{
					f(0) += gPtr[offset*ordlen + k] * hweights0[k];
					f(1) += gPtr[offset*ordlen + k] * hweights1[k];
					f(2) += gPtr[offset*ordlen + k] * hweights2[k];
				}
				for (int k = 0; k < ordlen; k++)
				{
					f(0) += bPtr[offset*ordlen + k] * hweights0[k];
					f(1) += bPtr[offset*ordlen + k] * hweights1[k];
					f(2) += bPtr[offset*ordlen + k] * hweights2[k];
				}
				f /= 3.0;
				Eigen::Vector3d normal = LInverse * f;
				temp[offset] = vcg::Point3f(normal(0), normal(1), normal(2));
				temp[offset].Normalize();
			}
		}
		normals.setLevel(temp, mipMapSize[level].width()*mipMapSize[level].height(), level);
	}
	

	return 0;

}

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(argv[0]);

  //Prepare files to run Kyle's 1DSFM Code
  
  FILE* file1 = fopen("../output/cc.txt", "w"); 
  if(file1 == NULL) {
    printf("\nCould not open CC file to write");
    return -1;
  }

  FILE* file2 = fopen("../output/EGs.txt", "w");
  if(file2 == NULL) {
    printf("\nCould not open EG file to write");
    return -1;
  }

  FILE* file3 = fopen("../output/coords.txt", "w");
  if(file2 == NULL) {
    printf("\nCould not open coords file to write");
    return -1;
  }
  
  FILE* file4 = fopen("../output/verified_matches.txt", "w");
  if(file4 == NULL) {
    printf("\nCould not open tracks file to write");
    return -1;
  }

  /*
  for(int i=0; i < numKeys; i++) {
    fprintf(file1,"%d\n",i);
  }*/


  vector<string> imnames;
  vector<CameraIntrinsicsPrior> cps;
  vector<ImagePairMatch> matches;

  bool s0 = ReadMatchesAndGeometry("../data/view_graph.bin",&imnames,&cps,&matches);
  if(s0) {
    printf("\nSuccessfully Read Matches And Geo, matches size %d", matches.size());

    for(int i=0; i < matches.size(); i++) {
    //for(int i=0; i < 5; i++) {
      int image1 = matches[i].image1_index;
      int image2 = matches[i].image2_index;

      TwoViewInfo currPairInfo = matches[i].twoview_info; 
      double focal_length1 = currPairInfo.focal_length_1;
      double focal_length2 = currPairInfo.focal_length_2;

      Eigen::Vector3d rotation = currPairInfo.rotation_2;
      Eigen::Vector3d position = currPairInfo.position_2;

      double angleMag = rotation.norm();
      Eigen::Vector3d axisVec = rotation.normalized();

      Eigen::AngleAxisd angax_rotation(angleMag, axisVec);
      Eigen::Matrix3d rotationMat = angax_rotation.toRotationMatrix();

      //std::cout << "Matrix " << i << ", rows" << rotationMat.rows() << ", cols" << rotationMat.cols() << endl;
      //std::cout << rotationMat;

      fprintf(file2, "%d %d ", image1, image2);
      for(int r= 0; r < 3; r++) {
        Eigen::Vector3d row_i = rotationMat.row(r);
        fprintf(file2, "%f %f %f ", row_i[0], row_i[1], row_i[2]);
      } 
      fprintf(file2, "%f %f %f\n", position[0], position[1], position[2]);
    }

    fclose(file1);
    fclose(file2);
    fclose(file3);
    fclose(file4);
  } else { 
    printf("\nCould not read bin file");
    return -1;
  }


  return 0;
}

//.........这里部分代码省略.........
      fscanf(fp,"%d %d",&matchIdx1, &matchIdx2);
      float x1 = keysInfo[img1][matchIdx1].x;
      float y1 = keysInfo[img1][matchIdx1].y; 

      float x2 = keysInfo[img2][matchIdx2].x; 
      float y2 = keysInfo[img2][matchIdx2].y; 

      Feature f1(x1, y1);
      Feature f2(x2, y2);
      FeatureCorrespondence corres;
      corres.feature1 = f1;
      corres.feature2 = f2;
      featCorrs.push_back(corres);     

      matchIdxPairs.push_back(make_pair(matchIdx1, matchIdx2));
    }
  
    vector<int> inliers;
    TwoViewInfo currPairInfo;
    VerifyTwoViewMatchesOptions options;
    bool status = VerifyTwoViewMatches(options,
        camPriors[img1], camPriors[img2],
        featCorrs,&currPairInfo,&inliers);

    if(status) {
      printf("\nSuccessfully Verified Matches");
      printf("\nFocal Length 1 = %f, Focal Length 2 = %f", camPriors[img1].focal_length.value, camPriors[img2].focal_length.value);
      printf("\nFocal Length 1 = %f, Focal Length 2 = %f", currPairInfo.focal_length_1, currPairInfo.focal_length_2);

      for(int inl_id=0; inl_id < inliers.size(); inl_id++) {
        inlFeatCorrs.push_back( featCorrs[inliers[inl_id]] ); 
      }

      ImagePairMatch imPair;
      imPair.image1_index = img1;
      imPair.image2_index = img2; 
      imPair.twoview_info = currPairInfo;
      imPair.correspondences = inlFeatCorrs;
      matches.push_back(imPair);

      int numInl = inliers.size();
      //      fprintf(file4, "%d\n", inliers.size());
      for(int j=0; j < numInl; j++) {
        pair<int,int> p = matchIdxPairs[inliers[j]];
        fprintf(file4, "2 %d %d %d %d\n", img1, p.first, img2, p.second); 
      }
    }
  }
  
  bool status = WriteMatchesAndGeometry("view_graph.bin", view_names, camPriors, matches);
  if(!status) {
    printf("\nSuccessfully written view-graph file");
  }
  
  // Write remaining files for Kyle's 1dsfm
  for(int i=0; i < numKeys; i++) {
    fprintf(file1,"%d\n",i);
    fprintf(file3, "#index = %d, name = %s, keys = %d ",
        i, imgList.getImageName(i).c_str(), numFeatures[i]); 
    fprintf(file3, "px = %.1f, py = %.1f, focal = %.3f\n",
        halfWidth[i], halfHeight[i], focalLengths[i]); 

    for(int j=0; j < numFeatures[i]; j++) {
      fprintf(file3, "%d %f %f 0 0 %d %d %d\n",
          j, keysInfo[i][j].x, keysInfo[i][j].y,
          0,0,0); 
    }
  }

  for(int i=0; i < matches.size(); i++) {
    int image1 = matches[i].image1_index;
    int image2 = matches[i].image2_index;

    TwoViewInfo currPairInfo = matches[i].twoview_info; 
    double focal_length1 = currPairInfo.focal_length_1;
    double focal_length2 = currPairInfo.focal_length_2;

    Eigen::Vector3d rotation = currPairInfo.rotation_2;
    Eigen::Vector3d position = currPairInfo.position_2;

    double angleMag = rotation.norm();
    Eigen::Vector3d axisVec = rotation.normalized();

    Eigen::AngleAxisd angax_rotation(angleMag, axisVec);
    Eigen::Matrix3d rotationMat = angax_rotation.toRotationMatrix();

    fprintf(file2, "%d %d ", image1, image2);
    for(int r= 0; r < 3; r++) {
      Eigen::Vector3d row_i = rotationMat.row(r);
      fprintf(file2, "%f %f %f ", row_i[0], row_i[1], row_i[2]);
    } 
    fprintf(file2, "%f %f %f\n", position[0], position[1], position[2]);
  }

  fclose(file1);
  fclose(file2);
  fclose(file3);
  fclose(file4);
  return 0;
}