当前位置: 首页>>代码示例>>C++>>正文

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

本文整理汇总了C++中Matrix4d::block方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix4d::block方法的具体用法？C++ Matrix4d::block怎么用？C++ Matrix4d::block使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Matrix4d的用法示例。


在下文中一共展示了Matrix4d::block方法的6个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: skew

Matrix6d adjoint_se3(Matrix4d T){
    Matrix6d AdjT = Matrix6d::Zero();
    Matrix3d R = T.block(0,0,3,3);
    AdjT.block(0,0,3,3) = R;
    AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ;
    AdjT.block(3,3,3,3) = R;
    return AdjT;
}
开发者ID:rubengooj，项目名称:StVO-PL，代码行数:8，代码来源:auxiliar.cpp

示例2: 

Matrix4d inverse_se3(Matrix4d T){
    Matrix4d Tinv = Matrix4d::Identity();
    Matrix3d R;
    Vector3d t;
    t = T.block(0,3,3,1);
    R = T.block(0,0,3,3);
    Tinv.block(0,0,3,3) =  R.transpose();
    Tinv.block(0,3,3,1) = -R.transpose() * t;
    return Tinv;
}
开发者ID:rubengooj，项目名称:StVO-PL，代码行数:10，代码来源:auxiliar.cpp

示例3: main

int main(void)
{
   const int n =6; // 4I , 2 lambda
   int info;

   // system params
   Coil coil;
   Magnet magnet;
   coil.d = .0575;
   coil.R = 0.075;

   magnet.x =  0.01;
   magnet.y =  0.0;
   magnet.Fx = 1 * pow(10,-8);
   magnet.Fy = 1.* pow(10,-8);
   magnet.gamma = 35.8 * pow(10,-6);
   double th = -19 * PI/180; // convert angle in degrees to radians

   double Bmag = 10. * pow(10,-6);
   double Bx = Bmag * cos(th);
   double By = Bmag * sin(th);

   Vector2d mvec(Bx,By);
   mvec = magnet.gamma/sqrt(Bx * Bx + By * By)*mvec;

   cout << "mvec" << endl << mvec << endl;

   MatrixXd Bmat = computeBmat(magnet.x,magnet.y,coil.R,coil.d);
   cout << "Bmat" << endl << Bmat << endl;
   //MatrixXd 

   Matrix4d A;
   cout << "A" << endl << A << endl;

   A.block(0,0,2,4) = Bmat;
   cout << "A row 1-2" << endl << A << endl;
   A.block(2,0,1,4) = mvec.transpose() * Dx(magnet.x,magnet.y,coil.R,coil.d);
   A.block(3,0,1,4) = mvec.transpose() * Dy(magnet.x,magnet.y,coil.R,coil.d);
   cout << "A " << endl << A << endl;

   cout << "A inverse is:" << endl << A.inverse() << endl;

   Vector4d BF;
   BF << Bx,By,magnet.Fx,magnet.Fy;
   cout << "BF: " << endl << BF << endl;
   Vector4d Isolve = A.inverse() * BF;

   cout << "Isolve: " << endl << Isolve << endl;
   return 0;
}
开发者ID:dwong229，项目名称:catkinpkg_plane_camera_magnet，代码行数:50，代码来源:test_solveI_desiredBF.cpp

示例4: if

Vector6d logmap_se3(Matrix4d T){
    Matrix3d R, Id3 = Matrix3d::Identity();
    Vector3d Vt, t, w;
    Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
    Vector6d x;
    Vt << T(0,3), T(1,3), T(2,3);
    w  << 0.f, 0.f, 0.f;
    R = T.block(0,0,3,3);
    double cosine = (R.trace() - 1.f)/2.f;
    if(cosine > 1.f)
        cosine = 1.f;
    else if (cosine < -1.f)
        cosine = -1.f;
    double sine = sqrt(1.0-cosine*cosine);
    if(sine > 1.f)
        sine = 1.f;
    else if (sine < -1.f)
        sine = -1.f;
    double theta  = acos(cosine);
    if( theta > 0.000001 ){
        w_hat = theta*(R-R.transpose())/(2.f*sine);
        w = skewcoords(w_hat);
        Matrix3d s;
        s = skew(w) / theta;
        V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
    }
    t = V.inverse() * Vt;
    x.head(3) = t;
    x.tail(3) = w;
    return x;
}
开发者ID:rubengooj，项目名称:StVO-PL，代码行数:31，代码来源:auxiliar.cpp

示例5: der_logarithm_map

MatrixXd der_logarithm_map(Matrix4d T)
{

    MatrixXd dlogT_dT = MatrixXd::Zero(6,12);

    // Approximate derivative of the logarithm_map wrt the transformation matrix
    Matrix3d L1 = Matrix3d::Zero();
    Matrix3d L2 = Matrix3d::Zero();
    Matrix3d L3 = Matrix3d::Zero();
    Matrix3d Vinv = Matrix3d::Identity();
    Vector6d x = logmap_se3(T);

    // estimates the cosine, sine, and theta
    double b;
    double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 );
    if(cos_ > 1.f)
        cos_ = 1.f;
    else if (cos_ < -1.f)
        cos_ = -1.f;
    double theta  = acos(cos_);
    double theta2 = theta*theta;
    double sin_   = sin(theta);
    double cot_   = 1.0 / tan( 0.5*theta );
    double csc2_  = pow( 1.0/sin(0.5*theta) ,2);

    // if the angle is small...
    if( cos_ > 0.9999 )
    {
        b = 0.5;
        L1(1,2) = -b;
        L1(2,1) =  b;
        L2(0,2) =  b;
        L2(2,0) = -b;
        L3(0,1) = -b;
        L3(1,0) =  b;
        // form the full derivative
        dlogT_dT.block(3,0,3,3) = L1;
        dlogT_dT.block(3,3,3,3) = L2;
        dlogT_dT.block(3,6,3,3) = L3;
        dlogT_dT.block(0,9,3,3) = Vinv;
    }
    // if not...
    else
    {
        // rotation part
        double k;
        Vector3d a;
        a(0) = T(2,1) - T(1,2);
        a(1) = T(0,2) - T(2,0);
        a(2) = T(1,0) - T(0,1);
        k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) );
        a = k * a;
        L1.block(0,0,3,1) = a;
        L2.block(0,1,3,1) = a;
        L3.block(0,2,3,1) = a;
        // translation part
        Matrix3d w_skew = skew( x.tail(3) );
        Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3);
        Vinv  = Vinv.inverse().eval();
        // dVinv_dR
        Vector3d t;
        Matrix3d B, skew_t;
        MatrixXd dVinv_dR(3,9);
        t = T.block(0,3,3,1);
        skew_t = skew( t );
        // - form a
        a =  (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t
            + ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t;
        // - form B
        Vector3d w;
        Matrix3d dw_dR;
        w = x.tail(3);
        dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2);
        dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2);
        dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1);
        B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2));
        // - form dVinv_dR
        dVinv_dR.col(0) = a;
        dVinv_dR.col(1) = -B.col(2);
        dVinv_dR.col(2) = B.col(1);
        dVinv_dR.col(3) = B.col(2);
        dVinv_dR.col(4) = a;
        dVinv_dR.col(5) = -B.col(0);
        dVinv_dR.col(6) = -B.col(1);
        dVinv_dR.col(7) = B.col(0);
        dVinv_dR.col(8) = a;
        // form the full derivative
        dlogT_dT.block(3,0,3,3) = L1;
        dlogT_dT.block(3,3,3,3) = L2;
        dlogT_dT.block(3,6,3,3) = L3;
        dlogT_dT.block(0,9,3,3) = Vinv;
        dlogT_dT.block(0,0,3,9) = dVinv_dR;
    }

    return dlogT_dT;

}
开发者ID:rubengooj，项目名称:StVO-PL，代码行数:97，代码来源:auxiliar.cpp

示例6: main

int main()
{
	//特徴点の座標
	double fpos[8][3]={	-1000,     0,	 5000,
                       1000,     0,	 5000,
                          0,     0,	 7000,
									        0,  2000, 10000,
										  -1000,  2000, 10000,
									        0,  3000, 10000,
									     1000,  2000, 10000,
											    0,  1000, 10000};

	//特徴点オブジェクト生成
	feature feat[8];

	//特徴点座標設定
	for(int i=0;i<8;i++){
		feat[i].setPos(fpos[i][0],fpos[i][1],fpos[i][2]);
	}

	//回転座標変換行列計算
	Vector3d axisx;
	axisx << 1,0,0;
	Vector3d axisy;
	axisy << 0,1,0;
	Vector3d axisz;
	axisz << 0,0,1;	
	Matrix3d rotx; 
	rotx = AngleAxisd(0.1,axisx);
	Matrix3d roty;
	roty = AngleAxisd(0.0,axisy);
	Matrix3d rotz;
	rotz = AngleAxisd(0.0,axisz);
	Matrix3d rot = rotz*roty*rotx;

	PRINT_MAT(rot);
	double tx=0.0,ty=2000.0,tz=0.0;
	Matrix4d T;
	T<< 0,0,0,-tx,
			0,0,0,-ty,
			0,0,0,-tz,
			0,0,0,1;
	PRINT_MAT(T);

	T.block(0,0,3,3)=rot;
	PRINT_MAT(T);
	
	double f=0.5,ox=320.0,oy=240.0,kx=10.0,ky=10.0;
	Matrix<double,3,4> K;
	K<< f*kx,    0, ox, 0,
			   0, f*ky, oy, 0,
			   0,    0,  1.0, 0;
	PRINT_MAT(K);
	
	Matrix<double,3,4> P;
	P=K*T;

	PRINT_MAT(P);

	Vector4d fp;
	Vector3d img;
	fp<<0.0,2000.0,10000.0,1.0;
	

	img=P*fp;

	PRINT_MAT(img);

  //画面上の座標表示
	printf("%f,%f\n",img(0)/img(2),img(1)/img(2));
	
	
}
开发者ID:kouhei1970，项目名称:monoSLAM-by-particlefilter，代码行数:73，代码来源:monoslam.cpp


注：本文中的Matrix4d::block方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。