本文整理汇总了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;
}
示例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;
}
示例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));
}
}
示例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;
}
示例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);
}
示例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);
}
}
示例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();
}
示例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;
}
示例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();
}
示例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;
}
示例11: rotateAroundAxisDnD
void SimObject::rotateAroundAxisDnD(double angle, Vector3d axis)
{
Matrix3d m;
axis.normalize();
m.setRotationAroundAxis(axis, angle);
rotateDnD(m, position);
}
示例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;
}
示例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;
}
}
示例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]);
}
示例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);
}
}
}