C++ Matrix2d类代码示例

 //define parameter computation
 virtual void compute_param(){
   Matrix2d M;
   Vector2d b, *point, param;
   list<Vector2d*>::iterator obs;
   
   M << 0.0, 0.0, 0.0, 0.0;
   b << 0.0, 0.0;
   
   //check if we have enough data in fit_set
   int ndata = (int)fit_set.size();
   if (ndata >= nDataMin) {
     //do linear least-squares fit
     for (obs = fit_set.begin(); obs != fit_set.end(); obs++) {
       point = *obs;
       M(0,0) += 1;
       M(0,1) += (*point)(0);
       M(1,0) += (*point)(0);
       M(1,1) += (*point)(0)*(*point)(0);
       b(0)   += (*point)(1);
       b(1)   += (*point)(0)*(*point)(1);
     }
     //calculate parameters
     param = M.inverse()*b;
     intercept = param(0);
     slope     = param(1);
   }
 };

//Check for passes through the left and right wall 
vector<stick> make_ghost_lr( const vector<stick> &m, double LStick, int NumberMikado)
{
  std::vector<stick> GhostLR;
  for (int i=0;i<NumberMikado;i++){
    if(m[i].th!=pi/2 || m[i].th!=3*pi/2){ // Check here for the left wall 
      stick ghostRowLR;
      Matrix2d A; //The matrix to solve with
      Vector2d b,b2; // A*st=b
      Vector2d st,st2;
 
      A<<-cos(m[i].th),0,-sin(m[i].th),1; 
      b<<m[i].x,m[i].y;
      b2<<m[i].x-1,m[i].y;
      st=A.lu().solve(b);
      st2=A.lu().solve(b2);
      if((st(0)>0 && st(0)<LStick) && (st(1)>0&&st(1)<1)){ //If mikado passes throug wall make ghost mikado 
        ghostRowLR=m[i];
        ghostRowLR.x=ghostRowLR.x+1;
        ghostRowLR.wlr=ghostRowLR.wlr-1;
        GhostLR.push_back(ghostRowLR); 
      }
      if((st2(0)>0&&st2(0)<LStick)&&(st2(1)>0&&st2(1)<1)){ //Now do the same for the upper wall
        ghostRowLR=m[i];
        ghostRowLR.x=ghostRowLR.x-1;
        ghostRowLR.wlr=ghostRowLR.wlr+1;
        GhostLR.push_back(ghostRowLR);
      }
    }
  }
  return GhostLR;
}

Vector2d leastSquareSolverCalib::pointAlignerLoop( Vector2d &x,  MatrixXd &Z, int iterations,  Vector2d &result)
{
    result=x;
    MatrixXd H(2,2);
    H.setZero();

    MatrixXd b(2,1);
    b.setZero();

    Vector2d X;
    std::cout << Z.transpose()<< std::endl;
    for(int i = 0; i < iterations; i++){
        std::cout<<"iteration "<<i <<std::endl;
        X=x;
        for(int j=0;j<Z.cols();j++){

            Matrix2d J = computeJacobian(j,Z);
            Vector2d e=computeError(j,Z);
            std:: cout << "error: "<< e<< std::endl<<std::endl;
            H+=J.transpose()*J;
            b+=J.transpose()*e;
        }
    }
    Vector2d dx;
    std::cout << "H: "<<std::endl<<H<<std::endl<<std::endl<<std::endl;
    std::cout << "b: "<<std::endl<<b<<std::endl;
    LDLT<MatrixXd> ldlt(-H);
    dx=ldlt.solve(b); // using a LDLT factorizationldlt;
    return dx;

}

//Check for passes through the down and up wall;
vector<stick> make_ghost_ud(const vector<stick> &m, double LStick, int NumberMikado){
  vector<stick> GhostUD;
  for(int i=0;i<NumberMikado;i++){
    if(m[i].th!=0||m[i].th!=pi){
      stick ghostRowUD;
      Matrix2d A;
      Vector2d b,b2;
      Vector2d st, st2;
      
      A<<-cos(m[i].th),1,-sin(m[i].th),0;
      b<<m[i].x,m[i].y;
      b2<<m[i].x,m[i].y-1;
      st=A.lu().solve(b);
      st2=A.lu().solve(b2);
      if((st(0)>0&&st(0)<LStick)&&(st(1)>0&&st(1)<1)){
	ghostRowUD=m[i];
	ghostRowUD.y=ghostRowUD.y+1;
	ghostRowUD.wud=ghostRowUD.wud-1;
	GhostUD.push_back(ghostRowUD);
	}
      if((st2(0)>0&&st2(0)<LStick)&&(st2(1)>0&&st2(1)<1)){
	ghostRowUD=m[i];
	ghostRowUD.y=ghostRowUD.y-1;
	ghostRowUD.wud=ghostRowUD.wud+1;
	GhostUD.push_back(ghostRowUD);
	}
     }
    
  }
  return GhostUD;
}

MatrixXd Utils::calculateHomographyMatrixFromFiveOrtoghonalLines(QList<Line*> firstOrtoghonalLines, QList<Line*> secondOrthogonalLines,
                     QList<Line*> thirdOrthogonalLines, QList<Line*> fourthOrthogonalLines,
                     QList<Line*> fifthOrthogonalLines)
{
    // A * x = b.
    MatrixXd A(5, 6);
    MatrixXd b(5, 1);
    MatrixXd x(5, 1);

    Vector3d l1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(0));
    Vector3d m1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(1));
    Vector3d l2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(0));
    Vector3d m2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(1));
    Vector3d l3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(0));
    Vector3d m3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(1));
    Vector3d l4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(0));
    Vector3d m4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(1));
    Vector3d l5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(0));
    Vector3d m5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(1));

    b << -l1(1)*m1(1), -l2(1)*m2(1), -l3(1)*m3(1), -l4(1)*m4(1), -l5(1)*m5(1);
    A << l1(0)*m1(0), (l1(0)*m1(1)+l1(1)*m1(0))/2, l1(1)*m1(1), (l1(0)*m1(2)+l1(2)*m1(0))/2, (l1(1)*m1(2)+l1(2)*m1(1))/2, l1(2)*m1(2),
         l2(0)*m2(0), (l2(0)*m2(1)+l2(1)*m2(0))/2, l2(1)*m2(1), (l2(0)*m2(2)+l2(2)*m2(0))/2, (l2(1)*m2(2)+l2(2)*m2(1))/2, l2(2)*m2(2),
         l3(0)*m3(0), (l3(0)*m3(1)+l3(1)*m3(0))/2, l3(1)*m3(1), (l3(0)*m3(2)+l3(2)*m3(0))/2, (l3(1)*m3(2)+l3(2)*m3(1))/2, l3(2)*m3(2),
         l4(0)*m4(0), (l4(0)*m4(1)+l4(1)*m4(0))/2, l4(1)*m4(1), (l4(0)*m4(2)+l4(2)*m4(0))/2, (l4(1)*m4(2)+l4(2)*m4(1))/2, l4(2)*m4(2),
         l5(0)*m5(0), (l5(0)*m5(1)+l5(1)*m5(0))/2, l5(1)*m5(1), (l5(0)*m5(2)+l5(2)*m5(0))/2, (l5(1)*m5(2)+l5(2)*m5(1))/2, l5(2)*m5(2);

   x = A.colPivHouseholderQr().solve(b);

   x/=x(2);

   Matrix3d C;
   C << x(0), x(1)/2, x(3)/2,
        x(1)/2, x(2), x(4)/2,
        x(3)/2, x(4)/2, 1;

   Matrix2d kkt;
   kkt << C(0,0), C(0,1),
          C(1,0), C(1,1);

   MatrixXd vKKt(1,2);
   vKKt << C(2,0), C(2,1);

   MatrixXd V(1,2);
   V = vKKt * kkt.inverse();

   LLT<MatrixXd> llt(kkt);
   MatrixXd U = llt.matrixU();

   MatrixXd J (3,3);
   J << U(0,0), U(0,1),0, U(1,0), U(1,1),0, V(0), V(1), 1;

   return J;
}

template <typename T> std::ostream& operator << (std::ostream &OutStream, const Matrix2d<T> &MatrixObj)
{
	Matrix2d<T> *pMatrixTmp =const_cast<Matrix2d<T> *>( &MatrixObj );
	for(int i = 0; i < pMatrixTmp->getMatrix2dRow(); i++){
		OutStream << "|"<< " ";
		for(int j = 0; j < pMatrixTmp->getMatrix2dColumn(); j++){
			OutStream << *(pMatrixTmp->getMatrix2dData() + i*pMatrixTmp->getMatrix2dColumn() + j)<< " ";
		}
		OutStream << "|" << std::endl;
	}

	return OutStream;
}

bool depthFromTriangulation(
    const SE3& T_search_ref,
    const Vector3d& f_ref,
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotationMatrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);
  return true;
}

/** 
 * This method is used to transpose the matrix
 * @author Razmik Avetisyan
 * @return Result of transpose operation
 */
Matrix2d Matrix2d::transpose()
{
	Matrix2d data = m;
	GLdouble *tmp = data.get();
	int swap;

    for(int i = 0; i < 3; i++)
	   for(int j = i+1; j < 3;j++){
			swap = tmp[i*3+j];
			tmp[i*3+j] = tmp[j*3+i];
			tmp[j*3+i] = swap;
	   } 

	return data;
}

double Triangle<ConcreteShape>::estimatedElementRadius() {
  Matrix2d invJ;
  double detJ;

  std::tie(invJ, detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
  Matrix2d J = invJ.inverse();
  VectorXcd eivals = J.eigenvalues();

  // get minimum h (smallest direction)
  Vector2d eivals_norm;
  for(int i=0;i<2;i++) {
    eivals_norm(i) = std::norm(eivals[i]);
  }
  return eivals_norm.minCoeff();
  
}

void warpAffine(
    const Matrix2d& A_cur_ref,
    const cv::Mat& img_ref,
    const Vector2d& px_ref,
    const int level_ref,
    const int search_level,
    const int halfpatch_size,
    uint8_t* patch)
{
  const int patch_size = halfpatch_size*2 ;
  const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
  if(isnan(A_ref_cur(0,0)))
  {
    printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
    return;
  }

  // Perform the warp on a larger patch.
  uint8_t* patch_ptr = patch;
  const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
  for (int y=0; y<patch_size; ++y)
  {
    for (int x=0; x<patch_size; ++x, ++patch_ptr)
    {
      Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
      px_patch *= (1<<search_level);
      const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
      if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
        *patch_ptr = 0;
      else
        *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
    }
  }
}

MatrixXd Triangle<ConcreteShape>::buildStiffnessMatrix(VectorXd velocity) {

  Matrix2d invJ;
  double detJ;
  std::tie(invJ,detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
  //Jinv= rx, sx,
  //      rz, sz;
  auto drdx = invJ(0,0);
  auto dsdx = invJ(0,1);
  auto drdz = invJ(1,0);
  auto dsdz = invJ(1,1);

  // save for later reuse
  mInvJac = invJ;
  mInvJacT_x_invJac = invJ.transpose() * invJ;
  mInvJacT = invJ.transpose();
  mDetJac = detJ;
  
  // build material on all nodes
  MatrixXd elementStiffnessMatrix(mNumIntPnt,mNumIntPnt);
    
  // interpolate velocity at all nodes
  // for(int i=0;i<mNumIntPnt;i++) {
  //   auto r = mIntegrationCoordinates_r[i];
  //   auto s = mIntegrationCoordinates_s[i];
  //   velocity(i) = interpolateAtPoint(r,s).dot(mMaterialVelocityAtVertices);
  // }  
  
  // loop over matrix(i,j)
  for(int i=0;i<mNumIntPnt;i++) {
      
    VectorXd dPhi_dr_i = mGradientPhi_dr.row(i);
    VectorXd dPhi_ds_i = mGradientPhi_ds.row(i);
    auto dPhi_dx_i = dPhi_dr_i*drdx + dPhi_ds_i*dsdx;
    auto dPhi_dz_i = dPhi_dr_i*drdz + dPhi_ds_i*dsdz;
    for(int j=0;j<mNumIntPnt;j++) {
      VectorXd dPhi_dr_j = mGradientPhi_dr.row(j);
      VectorXd dPhi_ds_j = mGradientPhi_ds.row(j);
      auto dPhi_dx_j = dPhi_dr_j*drdx + dPhi_ds_j*dsdx;
      auto dPhi_dz_j = dPhi_dr_j*drdz + dPhi_ds_j*dsdz;
            
      elementStiffnessMatrix(i,j) = detJ*mIntegrationWeights.dot((velocity.array().pow(2) * dPhi_dx_i.array() * dPhi_dx_j.array()).matrix()) +
        detJ*mIntegrationWeights.dot((velocity.array().pow(2) * dPhi_dz_i.array() * dPhi_dz_j.array()).matrix());
    }
  }
  return elementStiffnessMatrix;
}

void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){
  ///////////////////////////////////
  // we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one

  CovarianceEstimator ce(_graph);

  ce.setVertices(vset);
  ce.setGauge(_lastVertex);
  
  ce.compute();

  assert(!_lastVertex->fixed() && "last Vertex is fixed");
  assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
  
  OptimizableGraph::VertexSet tmpvset = vset;
  for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    
    MatrixXd Pv = ce.getCovariance(vertex);
    Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
    SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();	
    Vector2d hxy (delta.translation().x(), delta.translation().y());
    double perceptionRange =1;
    if (hxy.x()-perceptionRange>0) 
      hxy.x() -= perceptionRange;
    else if (hxy.x()+perceptionRange<0)
      hxy.x() += perceptionRange;
    else
      hxy.x() = 0;

    if (hxy.y()-perceptionRange>0) 
      hxy.y() -= perceptionRange;
    else if (hxy.y()+perceptionRange<0)
      hxy.y() += perceptionRange;
    else
      hxy.y() = 0;
    
    double d2 = hxy.transpose() * Pxy.inverse() * hxy;
    if (d2 > 5.99)
      vset.erase(*it);
 
  }
  
}

IGL_INLINE void igl::frame_to_cross_field(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::MatrixXd& FF1,
  const Eigen::MatrixXd& FF2,
  Eigen::MatrixXd& X)
{
  using namespace Eigen;

  // Generate local basis
  MatrixXd B1, B2, B3;

  igl::local_basis(V,F,B1,B2,B3);

  // Project the frame fields in the local basis
  MatrixXd d1, d2;
  d1.resize(F.rows(),2);
  d2.resize(F.rows(),2);

  d1 << igl::dot_row(B1,FF1), igl::dot_row(B2,FF1);
  d2 << igl::dot_row(B1,FF2), igl::dot_row(B2,FF2);

  X.resize(F.rows(), 3);

	for (int i=0;i<F.rows();i++)
	{
		Vector2d v1 = d1.row(i);
		Vector2d v2 = d2.row(i);

    // define inverse map that maps the canonical axis to the given frame directions
		Matrix2d A;
		A <<    v1[0], v2[0],
            v1[1], v2[1];

		// find the closest rotation
		Eigen::JacobiSVD<Matrix<double,2,2> > svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
    Matrix2d C = svd.matrixU() * svd.matrixV().transpose();

    Vector2d v = C.col(0);
    X.row(i) = v(0) * B1.row(i) + v(1) * B2.row(i);
  }
}

void GiTransform::setModelTransform(const Matrix2d& mat)
{
    if (mat.isInvertible() && m_impl->matM2W != mat)
    {
        m_impl->matM2W = mat;
        m_impl->matW2M = m_impl->matM2W.inverse();
        m_impl->matD2M = m_impl->matD2W * m_impl->matW2M;
        m_impl->matM2D = m_impl->matM2W * m_impl->matW2D;
        m_impl->zoomChanged();
    }
}

bool GiGraphics::drawPath_(const GiContext* ctx, const MgPath& path, bool fill, const Matrix2d& matD)
{
    int n = path.getCount();
    if (n == 0 || isStopping())
        return false;
    
    const Point2d* pts = path.getPoints();
    const char* types = path.getTypes();
    Point2d ends, cp1, cp2;
    bool matsame = matD.isIdentity();

    rawBeginPath();

    for (int i = 0; i < n; i++) {
        switch (types[i] & ~kMgCloseFigure) {
        case kMgMoveTo:
            ends = matsame ? pts[i] : (pts[i] * matD);
            rawMoveTo(ends.x, ends.y);
            break;

        case kMgLineTo:
            ends = matsame ? pts[i] : (pts[i] * matD);
            rawLineTo(ends.x, ends.y);
            break;

        case kMgBezierTo:
            if (i + 2 >= n)
                return false;
            cp1 = matsame ? pts[i] : (pts[i] * matD);
            cp2 = matsame ? pts[i+1] : (pts[i+1] * matD);
            ends = matsame ? pts[i+2] : (pts[i+2] * matD);
            rawBezierTo(cp1.x, cp1.y, cp2.x, cp2.y, ends.x, ends.y);
            i += 2;
            break;

        case kMgQuadTo:
            if (i + 1 >= n)
                return false;
            cp1 = matsame ? pts[i] : (pts[i] * matD);
            ends = matsame ? pts[i+1] : (pts[i+1] * matD);
            rawQuadTo(cp1.x, cp1.y, ends.x, ends.y);
            i++;
            break;

        default:
            return false;
        }
        if (types[i] & kMgCloseFigure)
            rawClosePath();
    }

    return rawEndPath(ctx, fill);
}