void AnchoredRectangleHandler::initRectangle(const Eigen::VectorXd& Fw,
    double lambda, const Eigen::VectorXd& z, Eigen::VectorXd& shapeParamshat,
    Eigen::VectorXd& FOhphat, Eigen::VectorXd &FOqhat) {

  //Get the points
  Eigen::Vector3d m1(z[0], z[1], 1);
  Eigen::Vector3d m2(z[2], z[3], 1);
  Eigen::Vector3d m3(z[4], z[5], 1);
  Eigen::Vector3d m4(z[6], z[7], 1);

  Eigen::Vector3d Ft(Fw[0], Fw[1], Fw[2]);
  Eigen::Quaterniond Fq(Fw[3], Fw[4], Fw[5], Fw[6]);

  //compute normals
  double c2 = (m1.cross(m3).transpose() * m4)[0]
      / (m2.cross(m3).transpose() * m4)[0];
  double c3 = (m1.cross(m3).transpose() * m2)[0]
      / (m4.cross(m3).transpose() * m2)[0];

  Eigen::Vector3d n2 = c2 * m2 - m1;
  Eigen::Vector3d n3 = c3 * m4 - m1;

  //Compute rotation matrix columns
  Eigen::Vector3d R1 = _K.inverse() * n2;
  R1 = R1 / R1.norm();

  Eigen::Vector3d R2 = _K.inverse() * n3;
  R2 = R2 / R2.norm();

  Eigen::Vector3d R3 = R1.cross(R2);

  //Compute rotation from camera to object
  Eigen::Matrix3d R;
  R << R1, R2, R3;
  Eigen::Quaterniond FOq_e(R);

  // and initialize the of the object with respect to the anchor frame
  FOqhat << FOq_e.w(), FOq_e.x(), FOq_e.y(), FOq_e.z();

  // now initialize lower left corner homogeneous point
  FOhphat << z[0], z[1], 1.0;
  FOhphat = _K.inverse() * FOhphat;
  FOhphat(2) = 1.0 / lambda; // 1/d distance of the plane parallel to the image plane on which features are initialized.

  //Compute frame transaltion
  Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
  double ff = sqrt(
      (n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);

  //compute shape parameters
  Eigen::Vector3d X = _K * R1;
  Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;

  double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];

  //Write the results
  shapeParamshat << ff, w / lambda;

}
예제 #2
0
파일: Proof.cpp 프로젝트: diantaowang/zcash
CompressedG1::CompressedG1(curve_G1 point)
{
    if (point.is_zero()) {
        throw std::domain_error("curve point is zero");
    }

    point.to_affine_coordinates();

    x = Fq(point.X);
    y_lsb = point.Y.as_bigint().data[0] & 1;
}
예제 #3
0
//=============================== Parton Dissociation ===============================//
Double_t SigmaQuasiQuark(Double_t PJPsi, Double_t T)
{
  
  Double_t PqStep = 0.05;
  Double_t PqStart = 0.0;
  Double_t PqEnd = 10.0;
  int PqN =(int)((PqEnd - PqStart)/PqStep);
    
  Double_t CosThetaStep = 0.1;
  Double_t CosThetaStart = 1.0;
  Double_t CosThetaEnd = -1.0;
  int CosThetaN =(int)((CosThetaStart - CosThetaEnd)/CosThetaStep);
  
  Double_t SumPq=0;  
  for(int i =0; i <= PqN ; i++) {
    Double_t Pq = PqStart + i*PqStep;
    Double_t SumCosTheta=0;
    Double_t IntCosTheta =0;
    
    for(int k =0; k <= CosThetaN ; k++)  {
      Double_t CosTheta = CosThetaStart - k*CosThetaStep;
      SumCosTheta = SumCosTheta + FuncCosTheta(PJPsi, Pq, CosTheta, T);
  
      //cout<< FuncCosTheta(PJPsi, Pq, CosTheta, T)<<endl;   

    }//theta
    IntCosTheta = SumCosTheta*CosThetaStep;
    
    //cout<<IntCosTheta<<endl;

    SumPq = SumPq + Pq*Pq*Fq(Pq,T)*IntCosTheta;  
    //cout<<SumPq<<endl;
  }//Pq
  

  Double_t IntPq =  (SumPq*PqStep)/(4.0*pi2);
  //cout<<IntPq<<endl;
  return (IntPq*1000.0);

  
}