C++ Matrix2d::inverse方法代码示例

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

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

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

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

void TransformCmd::setPointW(int index, const MgMotion* sender)
{
    Point2d point = sender->point * sender->view->xform()->displayToWorld();
    
    if (index > 0) {
        float a = (point - _origin).angle2();
        float len = _origin.distanceTo(point);
        a = floorf(0.5f + a * _M_R2D / 5) * 5 * _M_D2R;
        len = floorf(0.5f + len / 5) * 5;
        _axis[index == 1 ? 0 : 1].setAngleLength(a, len);
        
        Matrix2d mat(_axis[0], _axis[1], _origin * sender->view->xform()->worldToModel());
        mat *= _xfFirst.inverse();
        mat = sender->view->shapes()->modelTransform() * mat;
        
        sender->view->xform()->setModelTransform(mat);
        sender->view->regen();
    } else {
        _origin = point;
        sender->view->redraw(true);
    }
}

void CFullRelPose::writeEdge(std::ostream & toroGraphFile, const bool b2d) const
{
	if(IS_DEBUG) CHECK(!hasPosition(), "Node is disconnected?? Or too bad");

	const double dLength = length();
	//double dVar = sqr(dLength/4);// scale.scaleVarHacked();
	double dVar = variance();
	dVar = std::max<double>(1e-5, dVar);

	normalisedPose.scale(dLength).write(toroGraphFile, b2d);

	Vector3d t, x_axis; x_axis.setZero();
	normalisedPose.t.asVector(t);

	if(b2d)
	{
		//double inf_ff, inf_fs, inf_ss, inf_rr, inf_fr, inf_sr;
		Vector2d t2d(t(0), t(2));
		Vector2d t2d_perp(t(2), -t(0));

		Matrix2d Q, LAMBDA;
		LAMBDA.setZero();

		Q << t2d, t2d_perp;

		LAMBDA(0,0) = dVar; //dVar may be zero
		LAMBDA(1,1) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());

		CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")

		const Matrix2d C = Q * LAMBDA * Q.transpose();
		const Matrix2d & I = C.inverse();
		//cout << I << endl;

		//THROW("Not complete...")
		double dRotInf = 1.0/sqr(normalisedPose.SD.relOrientationSD());
		toroGraphFile << I(0,0) << ' ' << I(1,0) << ' ' << I(1,1) << ' ' << dRotInf << " 0 0";

		return;
	}

	if(t(0) < 0.9) //check t isn't x axis aligned (x is arbitrary)
		x_axis(0) = 1;
	else
		x_axis(1) = 1;

	Matrix3d Q;

	if(t.sum() == 0) //Pure rotation. Should be arbitrary
	{
		Q.setIdentity();
	}
	else
	{
		Vector3d tperp1 = t.cross(x_axis);
		tperp1.normalize();

		Vector3d tperp2 = tperp1.cross(t);

		Q << t, tperp1, tperp2;
	}

	CHECK(isnan(Q.sum()), "writeEdge: nan")

	Matrix3d LAMBDA; LAMBDA.setZero();
	LAMBDA(0,0) = dVar; //dVar may be zero
	LAMBDA(1,1) = LAMBDA(2,2) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());

	CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")

	const Matrix3d C = Q * LAMBDA * Q.transpose();

	//std::cout << "Covariance: " << std::endl << C << std::endl;
	CHECK(isnan(C.sum()), "writeEdge: nan")

	Matrix3d Crpy; Crpy.setZero();
	Crpy.diagonal().setConstant(sqr(normalisedPose.SD.relOrientationSD()));
	Matrix<double, 6, 6> Cfull; Cfull << C, Matrix3d::Zero(), Matrix3d::Zero(), Crpy;

	CHECK(isnan(Cfull.sum()), "writeEdge: nan")

	if(Cfull.trace() < 0.0001)
	{
		std::cout << "Warning: Covariance matrix near-singular, adjusting diagonal\n";
		Cfull.diagonal().array() += 0.0001;
	}

	const Matrix<double, 6, 6> & Ifull = Cfull.inverse();
	//std::cout << "Information: " << std::endl << Ifull << std::endl;

	//cout << "Warning: Not inverting information matrix\n"; Yes the information mat does work a little better.
	//Possibly ok for 1 edge...if(IS_DEBUG) CHECK(Cfull.determinant()<0.0001, "CFullRelPose::writeEdge: Singular covariance matrix");

	CHECK(isnan(Ifull.sum()), "writeEdge: nan")

	for (int nRow = 0; nRow <6; nRow++)
	{
		for(int nCol = nRow; nCol < 6; nCol++)
			toroGraphFile << Ifull(nRow, nCol) << ' ';
	}
//.........这里部分代码省略.........