C++ Matrix3d::col方法代码示例

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

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

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

Vector4d gc_av_to_asd(Vector6d av) {
  Vector4d asd;

  Vector3d a = av.head(3);
  Vector3d x = av.tail(3);  // v
  Vector3d y = a.cross(x);  // n

//  Vector2d w(y.norm(), x.norm());
//  w /= w.norm();
//  asd(3) = asin(w(1));
  
  double depth = x.norm() / y.norm();
  // double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));

//  double quotient = depth / 0.5;
//  int integer_quotient = (int)quotient;
//  double floating_quotient = quotient - (double)integer_quotient;
//  depth = depth * floating_quotient;
//  double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);

// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);

// double sig_d = atan2(1.0, depth) / 4.0;
  double sig_d = 1.0 / exp(-depth);
  asd(3) = sig_d;
  // cout << "sig_d = " << sig_d << endl;

  // asd(3) = depth;

  // double sig_d = tan(1.0/depth);
  // double sig_d = tan(2.0/depth);
  // double sig_d = tan(2.0*depth);
  // double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
  // double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
  // double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
  // double sig_d = 1.0 / (1.0 + exp(-depth));

  x /= x.norm();
  y /= y.norm();
  Vector3d z = x.cross(y);

  Matrix3d R;
  R.col(0) = x;
  R.col(1) = y;
  R.col(2) = z;

  Vector3d aa = gc_Rodriguez(R);

  asd(0) = aa(0);
  asd(1) = aa(1);
  asd(2) = aa(2);

  return asd;
}

void Needle::updateTransformFromAttachment()
{
	if (isThreadAttached()) {
		const Matrix3d thread_rot = thread->rotationAtConstraint(constraint_ind);
		const Vector3d thread_pos = thread->positionAtConstraint(constraint_ind);
		const Matrix3d needle_rot = AngleAxisd(angle * M_PI/180.0, thread_rot.col(1)) * thread_rot;
		const Vector3d needle_pos = thread_pos - radius * (AngleAxisd(-angle * M_PI/180.0, needle_rot.col(1)) * needle_rot).col(2);
		setTransform(needle_pos, needle_rot);
	}
}

Matrix3d toMatrix3d(const btMatrix3x3& basis)
{
	Matrix3d rotation;
	btVector3 col0 = basis.getColumn(0);
	btVector3 col1 = basis.getColumn(1);
	btVector3 col2 = basis.getColumn(2);
	rotation.col(0) = toVector3d(col0);
	rotation.col(1) = toVector3d(col1);
	rotation.col(2) = toVector3d(col2);
	return rotation;
}

void Needle::getEndEffectorTransform(Vector3d& ee_pos, Matrix3d& ee_rot)
{
	ee_rot = rotation * rotation_offset.transpose();
	ee_rot.col(1) = ee_rot.col(2).cross(ee_rot.col(0));
	ee_rot.col(2) = ee_rot.col(0).cross(ee_rot.col(1));
	ee_rot.col(0).normalize();
	ee_rot.col(1).normalize();
	ee_rot.col(2).normalize();
	
	ee_pos = position - ee_rot*rotation_offset*position_offset;
}

inline Matrix3d d_Tinvpsi_d_psi(const SE3Quat & T, const Vector3d & psi) {
    Matrix3d R = T.rotation().toRotationMatrix();
    Vector3d x = invert_depth(psi);
    Vector3d r1 = R.col(0);
    Vector3d r2 = R.col(1);
    Matrix3d J;
    J.col(0) = r1;
    J.col(1) = r2;
    J.col(2) = -R*x;
    J*=1./psi.z();
    return J;
}

        // Original code from the ROS vslam package pe3d.cpp
        // uses the SVD procedure for aligning point clouds
        //   SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
        Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
        {
            using namespace Eigen;

            SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");

            Vector3d c0 = p0.rowwise().mean();
            Vector3d c1 = p1.rowwise().mean();

            Matrix3d H(Matrix3d::Zero());
            // subtract out
            // p0a -= c0;
            // p0b -= c0;
            // p0c -= c0;
            // p1a -= c1;
            // p1b -= c1;
            // p1c -= c1;

            // Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
            // 	p1c*p0c.transpose();
            for(int i = 0; i < p0.cols(); ++i)
            {
                H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
            }
      

            // do the SVD thang
            JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
            Matrix3d V = svd.matrixV();
            Matrix3d R = V * svd.matrixU().transpose();          
            double det = R.determinant();
        
            if (det < 0.0)
            {
                V.col(2) = V.col(2) * -1.0;
                R = V * svd.matrixU().transpose();
            }
            Vector3d tr = c0-R.transpose()*c1;    // translation 
      
            // transformation matrix, 3x4
            Matrix4d tfm(Matrix4d::Identity());
            //        tfm.block<3,3>(0,0) = R.transpose();
            //        tfm.col(3) = -R.transpose()*tr;
            tfm.topLeftCorner<3,3>() = R.transpose();
            tfm.topRightCorner<3,1>() = tr;
      
            return tfm;
      
        }

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

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

    Matrix4d TrfmRotateExpMap::getSecondDeriv(const Dof *q1, const Dof *q2){
        Vector3d q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());

        // derivative wrt which mDofs
        int j=-1, k=-1;
        for(unsigned int i=0; i<mDofs.size(); i++) {
            if(q1==mDofs[i]) j=i;
            if(q2==mDofs[i]) k=i;
        }
        assert(j!=-1);
        assert(k!=-1);
        assert(j>=0 && j<=2);
        assert(k>=0 && k<=2);

        Matrix3d R = utils::rotation::expMapRot(q);
        Matrix3d J = utils::rotation::expMapJac(q);
        Matrix3d Jjss = utils::makeSkewSymmetric(J.col(j));
        Matrix3d Jkss = utils::makeSkewSymmetric(J.col(k));
        Matrix3d dJjdkss = utils::makeSkewSymmetric(utils::rotation::expMapJacDeriv(q, k).col(j));

        Matrix3d d2Rdidj = (Jjss*Jkss + dJjdkss)*R;

        Matrix4d d2Rdidj4 = Matrix4d::Zero();
        d2Rdidj4.topLeftCorner(3,3) = d2Rdidj;

        return d2Rdidj4;
    }

int main(int, char**)
{
  cout.precision(3);
  Matrix3d m = Matrix3d::Identity();
m.col(1) = Vector3d(4,5,6);
cout << m << endl;

  return 0;
}

Vector6d gc_orth_to_av(Vector4d auth) {
  Vector6d av;

  double a = auth[0];
  double b = auth[1];
  double g = auth[2];
  double t = auth[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 = cos(t) / sin(t);
  av.head(3) = -R.col(2) * d;
  av.tail(3) = R.col(1);

//   Vector3d u1 = R.col(0);
//   Vector3d u2 = R.col(1);
// 
//   double sigma1 = cos(t);
//   double sigma2 = sin(t);
// 
//   double d = cos(t) / sin(t);
// 
// //  Vector3d n = sigma1 * u1;
// //  Vector3d v = sigma2 * u2;
//   Vector3d n = u1;
//   Vector3d v = u2;
//   av.head(3) = -v.cross(n) * d;
//   av.tail(3) = v;

  return av;
}

Vector4d gc_av_to_aid(Vector6d av) {

  Vector4d aid;
  Vector3d a = av.head(3);
  Vector3d x = av.tail(3);  // v
  Vector3d y = a.cross(x);  // n
  aid(3) = x.norm() / y.norm();
  x /= x.norm();
  y /= y.norm();
  Vector3d z = x.cross(y);

  Matrix3d R;
  R.col(0) = x;
  R.col(1) = y;
  R.col(2) = z;

  Vector3d aa = gc_Rodriguez(R);

  aid(0) = aa(0);
  aid(1) = aa(1);
  aid(2) = aa(2);

//  Vector4d aid;
//  Vector3d a = av.head(3);
//  Vector3d v = av.tail(3);  // v
//  Vector3d n = a.cross(v);  // n
//
//  aid(3) = v.norm() / n.norm();
//
//  Vector3d x = n / n.norm();
//  Vector3d y = v / v.norm();
//  Vector3d z = x.cross(y);
//
//  aid[0] = atan2( y(2), z(2) );
//  aid[1] = asin( - x(2) );
//  aid[2] = atan2( x(1), x(0) );

  return aid;
}