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


C++ Matrix3d类代码示例

本文整理汇总了C++中Matrix3d的典型用法代码示例。如果您正苦于以下问题:C++ Matrix3d类的具体用法?C++ Matrix3d怎么用?C++ Matrix3d使用的例子?那么, 这里精选的类代码示例或许可以为您提供帮助。


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

示例1: calcW

Matrix<double,7,1> Sim3
::log(const Sim3& sim3)
{
  Vector7d res;
  double scale = sim3.scale();

  Vector3d t = sim3.translation_;

  double theta;

  Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
  Vector3d omega = omega_sigma.head<3>();
  double sigma = omega_sigma[3];
  Matrix3d Omega = SO3::hat(omega);

  Matrix3d W = calcW(theta, sigma, scale, Omega);

  //Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
  Vector3d upsilon = W.partialPivLu().solve(t);

  res.segment(0,3) = upsilon;
  res.segment(3,3) = omega;
  res[6] = sigma;
  return res;
}
开发者ID:EI2012zyq,项目名称:OpenMVO,代码行数:25,代码来源:sim3.cpp

示例2: convToPose

geometry_msgs::Pose MoveitPlanningInterface::convToPose(Vector3f plane_normal, Vector3f major_axis, Vector3f centroid) {
	geometry_msgs::Pose pose;
	Affine3d Affine_des_gripper;
	Vector3d xvec_des,yvec_des,zvec_des,origin_des;
	
	float temp;
	temp = major_axis[0];
	major_axis[0] = major_axis[1];
	major_axis[1] = temp;

	Vector3f rotation = major_axis;
	major_axis[0] = (sqrt(3)/2)*rotation[0] - rotation[1]/2;
	major_axis[1] = (sqrt(3)/2)*rotation[1] + rotation[0]/2;

	Matrix3d Rmat;
	for (int i=0;i<3;i++) {
		origin_des[i] = centroid[i]; // convert to double precision
		zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
		xvec_des[i] = major_axis[i];
	}
//	origin_des[2]+=0.02; //raise up 2cm
	yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
	Rmat.col(0)= xvec_des;
	Rmat.col(1)= yvec_des;
	Rmat.col(2)= zvec_des;
	Affine_des_gripper.linear()=Rmat;
	Affine_des_gripper.translation()=origin_des;
	
	//convert des pose from Affine to geometry_msgs::PoseStamped
	pose = transformEigenAffine3dToPose(Affine_des_gripper);
	return pose;
}
开发者ID:TuZZiX,项目名称:ros_workspace,代码行数:32,代码来源:arm_planning_lib_moveit.cpp

示例3: TEST

TEST(Rotation, TestEulerAnglesAndMatrices) {
  // Randomly generate euler angles, convert to a matrix, then convert back.
  // Check that (1) the rotation matrix has det(R)==1, and (2), that we get the
  // same angles back.
  math::RandomGenerator rng(0);
  for (int ii = 0; ii < 1000; ++ii) {
    Vector3d e1;
    e1.setRandom();

    // Converting from rotation matrices to euler angles is only valid when phi,
    // theta, and psi are all < 0.5*PI. Otherwise the problem has multiple
    // solutions, and we can only return one of them with our function.
    e1 *= 0.5 * M_PI;

    Matrix3d R = EulerAnglesToMatrix(e1);
    EXPECT_NEAR(1.0, R.determinant(), 1e-8);

    Vector3d e2 = MatrixToEulerAngles(R);
    EXPECT_NEAR(0.0, S1Distance(e1(0), e2(0)), 1e-8);
    EXPECT_NEAR(0.0, S1Distance(e1(1), e2(1)), 1e-8);
    EXPECT_NEAR(0.0, S1Distance(e1(2), e2(2)), 1e-8);

    EXPECT_TRUE(R.isApprox(EulerAnglesToMatrix(e2), 1e-4));
  }
}
开发者ID:erik-nelson,项目名称:berkeley_sfm,代码行数:25,代码来源:test_rotation.cpp

示例4: logmap_se3

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: IK_SetTransform

void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
	IK_QSegment *qseg = (IK_QSegment *)seg;

	Vector3d mstart(start[0], start[1], start[2]);
	// convert from blender column major
	Matrix3d mbasis = CreateMatrix(basis[0][0], basis[1][0], basis[2][0],
	                               basis[0][1], basis[1][1], basis[2][1],
	                               basis[0][2], basis[1][2], basis[2][2]);
	Matrix3d mrest = CreateMatrix(rest[0][0], rest[1][0], rest[2][0],
	                              rest[0][1], rest[1][1], rest[2][1],
	                              rest[0][2], rest[1][2], rest[2][2]);
	double mlength(length);

	if (qseg->Composite()) {
		Vector3d cstart(0, 0, 0);
		Matrix3d cbasis;
		cbasis.setIdentity();
		
		qseg->SetTransform(mstart, mrest, mbasis, 0.0);
		qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
	}
	else
		qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
开发者ID:DarkDefender,项目名称:blender-npr-tess2,代码行数:25,代码来源:IK_Solver.cpp

示例6: EnvObject

Needle::Needle(const Vector3d& pos, const Matrix3d& rot, double degrees, double r, float c0, float c1, float c2, World* w, ThreadConstrained* t, int constrained_vertex_num)
	: EnvObject(c0, c1, c2, NEEDLE)
	, angle(degrees)
	, radius(r)
	, thread(t)
	, constraint(constrained_vertex_num)
	, world(w)
	, position_offset(0.0, 0.0, 0.0)
	, rotation_offset(Matrix3d::Identity())
{
	assert(((thread == NULL) && (constrained_vertex_num == -1)) || ((thread != NULL) && (constrained_vertex_num != -1)));
	updateConstraintIndex();
	
	assert(degrees > MIN_ANGLE);
	assert(r > MIN_RADIUS);

	double arc_length = radius * angle * M_PI/180.0;
	int pieces = ceil(arc_length/2.0);
  for (int piece=0; piece<pieces; piece++) {
 		Intersection_Object* obj = new Intersection_Object();
 		obj->_radius = radius/8.0;
  	i_objs.push_back(obj);
	}	

	if (thread != NULL) {
		Matrix3d new_rot = AngleAxisd(-M_PI/2.0, rot.col(0)) * (AngleAxisd((angle) * M_PI/180.0, rot.col(1)) * rot);
		setTransform(pos - radius * rot.col(1), new_rot);
	} else {
		setTransform(pos, rot);
	}
}
开发者ID:rll,项目名称:surgical,代码行数:31,代码来源:Needle.cpp

示例7: str

/// Constructs a vector-valued attribute with the given name
XMLAttrib::XMLAttrib(const std::string& name, const Matrix3d& matrix_value)
{  
  this->processed = false;
  this->name = name;
  std::ostringstream oss;

  // set the first value of the matrix
  oss << str(matrix_value(0,0));

  // for each row of the matrix
  for (unsigned j=0; j< matrix_value.rows(); j++)
  {
    // determine column iteration
    unsigned i = (j == 0) ? 1 : 0;

    // for each column of the matrix
    for (; i< matrix_value.columns(); i++)
      oss << " " << str(matrix_value(j,i));

    // separate rows with a semicolon
    oss << ";";
  }

  // get the string value
  this->value = oss.str();
}
开发者ID:PositronicsLab,项目名称:Moby,代码行数:27,代码来源:XMLTree.cpp

示例8: Distance

double Distance( const Conic& c1, const Conic& c2, double circle_radius )
{
  const Matrix3d Q = c1.Dual * c2.C;
  //  const double dsq = 3 - trace(Q)* pow(determinant(Q),-1.0/3.0);
  const double dsq = 3 - Q.trace()* 1.0 / cbrt(Q.determinant());
  return sqrt(dsq) * circle_radius;
}
开发者ID:stevenlovegrove,项目名称:fiducials,代码行数:7,代码来源:conics.cpp

示例9: A

MatrixXd Utils::calculateHomographyMatrix(vector<Vector3i> selectedPoints, vector<Vector3d> realWorldPoints)
{
    MatrixXd A(8, 8);
    MatrixXd B(8,1);

    for (uint i = 0; i < realWorldPoints.size(); i++)
    {
        double realWorldX = realWorldPoints.at(i).x();
        double realWorldY = realWorldPoints.at(i).y();
        int selectedPointX = selectedPoints.at(i).x();
        int selectedPointY = selectedPoints.at(i).y();

        A.row(i*2) << realWorldX, realWorldY, 1, 0, 0, 0, -realWorldX*selectedPointX, -realWorldY*selectedPointX;
        A.row(i*2 + 1) << 0, 0, 0, realWorldX, realWorldY, 1, -realWorldX*selectedPointY, -realWorldY*selectedPointY;

        B.row(i*2) << selectedPointX;
        B.row(i*2 +1) << selectedPointY;
    }
    //A  = A.inverse().eval();

    MatrixXd x = A.fullPivLu().solve(B);
    Matrix3d H;

    H << x(0), x(1), x(2), x(3), x(4), x(5), x(6), x(7), 1;

    return H.inverse();
}
开发者ID:flavioschuindt,项目名称:qt-persperctive-distortion-remotion,代码行数:27,代码来源:utils.cpp

示例10: gc_asd_to_av

Vector6d gc_asd_to_av(Vector4d asd) {
  Vector6d av;

  Vector3d aa = asd.head(3);

  // double d_inv = asd(3);

  // double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));

  // double sig_d_inv = -log(1.0/asd(3) - 1.0);
  // double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
  // double sig_d_inv = atan(asd(3)) / 2.0;
  // double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
  // double sig_d_inv = atan2(asd(3), 1.0);

  // double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
  
  // double sig_d_inv = tan(4.0 * asd(3));
  double sig_d_inv = log(asd(3));
  // cout << "sig_d_inv = " << sig_d_inv << endl;

  // double sig_d_inv = cos(asd(3)) / sin(asd(3));

  // double sig_d_inv = sin(asd(3)) / cos(asd(3));
  // double sig_d_inv = sin(asd(3)) / cos(asd(3));

  Matrix3d R = gc_Rodriguez(aa);

  // av.head(3) = R.col(2) / sig_d_inv;
  av.head(3) = R.col(2) * sig_d_inv;
  av.tail(3) = R.col(0);

  return av;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:34,代码来源:gc.cpp

示例11: rotateAroundAxisDnD

void SimObject::rotateAroundAxisDnD(double angle, Vector3d axis)
{
  Matrix3d m;
  axis.normalize();
  m.setRotationAroundAxis(axis, angle);
  rotateDnD(m, position);
}
开发者ID:alon,项目名称:bhuman2009fork,代码行数:7,代码来源:SimObject.cpp

示例12: gc_aid_to_av

Vector6d gc_aid_to_av(Vector4d aid) {

  Vector6d av;
  Vector3d aa = aid.head(3);
  double d = 1.0 / aid(3);
  Matrix3d R = gc_Rodriguez(aa);
  av.head(3) = R.col(2) * d;
  av.tail(3) = R.col(0);

//  Vector6d av;
//  double a = aid[0];
//  double b = aid[1];
//  double g = aid[2];
//  double t = aid[3];
//
//  double s1 = sin(a);
//  double c1 = cos(a);
//  double s2 = sin(b);
//  double c2 = cos(b);
//  double s3 = sin(g);
//  double c3 = cos(g);
//
//  Matrix3d R;
//  R <<
//      c2 * c3,   s1 * s2 * c3 - c1 * s3,   c1 * s2 * c3 + s1 * s3,
//      c2 * s3,   s1 * s2 * s3 + c1 * c3,   c1 * s2 * s3 - s1 * c3,
//      -s2,                  s1 * c2,                  c1 * c2;
//
//  double d = 1.0 / t;
//  av.head(3) = -R.col(2) * d;
//  av.tail(3) = R.col(1);

  return av;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:34,代码来源:gc.cpp

示例13: fromMatrix

void Angle::fromMatrix(const Matrix3d& matrix)
{
	assert(matrix.x() == 3 && matrix.y() == 3);
	double forward[3];
	double left[3];
	double up[3];

	forward[0] = matrix[0][0];
	forward[1] = matrix[1][0];
	forward[2] = matrix[2][0];
	left[0] = matrix[0][1];
	left[1] = matrix[1][1];
	left[2] = matrix[2][1];
	up[2] = matrix[2][2];

	double xyDist = sqrt(forward[0] * forward[0] + forward[1] * forward[1]);

	if (xyDist > 0.0001)
	{
		(*this)[PITCH] = RAD2DEG( atan2( -forward[2], xyDist ) );
		(*this)[YAW] = RAD2DEG( atan2( forward[1], forward[0] ) );
		(*this)[ROLL] = RAD2DEG( atan2( left[2], up[2] ) );
	}
	else
	{ // gimbal lock
		(*this)[PITCH] = RAD2DEG( atan2( -forward[2], xyDist ) );
		(*this)[YAW] = RAD2DEG( atan2( -left[0], left[1] ) );
		(*this)[ROLL] = 0;
	}
}
开发者ID:,项目名称:,代码行数:30,代码来源:

示例14: distPtTri

double distPtTri(Vector3d p, Matrix4d m){
    /// compute distance between a triangle and a point
    double s[4];
    Vector3d a,b,c,n;
    a << m(0,0), m(0,1), m(0,2);
    b << m(1,0), m(1,1), m(1,2);
    c << m(2,0), m(2,1), m(2,2);
    n << m(3,0), m(3,1), m(3,2);
    double k=(n-a).dot(a-p);
    if(k<0) return HUGE_VALF;
    s[0]=distPtLin(p,a,b);
    s[1]=distPtLin(p,b,c);
    s[2]=distPtLin(p,c,a);
    Matrix3d A;
    A << b(0)-a(0), c(0)-a(0), n(0)-a(0),
    b(1)-a(1), c(1)-a(1), n(1)-a(1),
    b(2)-a(2), c(2)-a(2), n(2)-a(2);
    Vector3d v = A.inverse()*(p-a);
    if(v(0)>0 && v(1)>0 && v(0)+v(1)<1){
        s[3]=k*k;
    }else{
        s[3] = HUGE_VALF;
    }
    return min(min(min(s[0],s[1]),s[2]),s[3]);
}
开发者ID:Leopardob,项目名称:CageDeformerMaya,代码行数:25,代码来源:cageDeformerARAP.cpp

示例15: A

/**
 * @brief CameraDirectLinearTransformation::rq3
 * Perform 3 RQ decomposition of matrix A and save them in matrix R and matrix Q
 * http://www.csse.uwa.edu.au/~pk/research/matlabfns/Projective/rq3.m
 * @param A
 * @param R
 * @param Q
 */
void CameraDirectLinearTransformation::rq3(const Matrix3d &A, Matrix3d &R, Matrix3d& Q)
{
    // Find rotation Qx to set A(2,1) to 0
    double c = -A(2,2)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
    double s = A(2,1)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
    Matrix3d Qx,Qy,Qz;
    Qx << 1 ,0,0, 0,c,-s, 0,s,c;
    R = A*Qx;
    // Find rotation Qy to set A(2,0) to 0
    c = R(2,2)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
    s = R(2,0)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
    Qy << c, 0, s, 0, 1, 0,-s, 0, c;
    R*=Qy;

    // Find rotation Qz to set A(1,0) to 0
    c = -R(1,1)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
    s =  R(1,0)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
    Qz << c ,-s, 0, s ,c ,0, 0, 0 ,1;
    R*=Qz;

    Q = Qz.transpose()*Qy.transpose()*Qx.transpose();
    // Adjust R and Q so that the diagonal elements of R are +ve
    for (int n=0; n<3; n++)
    {
        if (R(n,n)<0)
        {
            R.col(n) = - R.col(n);
            Q.row(n) = - Q.row(n);
        }
    }
}
开发者ID:CarloNicolini,项目名称:Volvux,代码行数:39,代码来源:Homography.cpp


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