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

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

  void EdgeProjectXYZ2UVQ::linearizeOplus()
  {

    VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
    SE3Quat T(vj->estimate());

    VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
    Vector3d xyz = vi->estimate();
    Vector3d xyz_trans = T.map(xyz);

    const double & x = xyz_trans[0];
    const double & y = xyz_trans[1];
    const double & z = xyz_trans[2];
    double z_sq = z*z;
    const double & Fx = vj->_focal_length(0);
    const double & Fy = vj->_focal_length(1);
    double dq_dz = -Fx/z_sq;
    double x_Fx_by_zsq = x*Fx/z_sq;
    double y_Fy_by_zsq = y*Fy/z_sq;


     Matrix3d R = T.rotation().toRotationMatrix();
    _jacobianOplusXi.row(0) = -Fx/z*R.row(0) + x_Fx_by_zsq*R.row(2);
    _jacobianOplusXi.row(1) = -Fy/z*R.row(1) + y_Fy_by_zsq*R.row(2);
    _jacobianOplusXi.row(2) =  -dq_dz*R.row(2);


    _jacobianOplusXj(0,0) =  x*y/z_sq *Fx;
    _jacobianOplusXj(0,1) = -(1+(x*x/z_sq)) *Fx;
    _jacobianOplusXj(0,2) = y/z *Fx;
    _jacobianOplusXj(0,3) = -1./z *Fx;
    _jacobianOplusXj(0,4) = 0;
    _jacobianOplusXj(0,5) = x/z_sq *Fx;


    _jacobianOplusXj(1,0) = (1+y*y/z_sq) *Fy;
    _jacobianOplusXj(1,1) = -x*y/z_sq *Fy;
    _jacobianOplusXj(1,2) = -x/z *Fy;
    _jacobianOplusXj(1,3) = 0;
    _jacobianOplusXj(1,4) = -1./z *Fy;
    _jacobianOplusXj(1,5) = y/z_sq *Fy;

    _jacobianOplusXj(2,0) = -y*dq_dz ;
    _jacobianOplusXj(2,1) = x*dq_dz;
    _jacobianOplusXj(2,2) = 0;
    _jacobianOplusXj(2,3) = 0;
    _jacobianOplusXj(2,4) = 0;
    _jacobianOplusXj(2,5) = -dq_dz ;

//    std::cerr << _jacobianOplusXi << std::endl;
//    std::cerr << _jacobianOplusXj << std::endl;

//    BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexSE3Expmap, false>::linearizeOplus();
//    std::cerr << _jacobianOplusXi << std::endl;
//    std::cerr << _jacobianOplusXj << std::endl;
  }

  void Camera::initializeViewPoint()
  {
    d->modelview.setIdentity();
    d->orthoScale = 1.0;
    if( d->parent == 0 )
        return;
    if( d->parent->molecule() == 0 )
        return;

    // if the molecule is empty, we want to look at its center
    // (which is probably at the origin, but who knows) from some distance
    // (here 20.0) -- this gives us some room to work PR#1964674
    if( d->parent->molecule()->numAtoms() < 2 &&
        d->parent->molecule()->OBUnitCell() == NULL)
    {
      d->modelview.translate(-d->parent->center() - Vector3d(0.0, 0.0, 20.0));
      return;
    }

    // if we're here, the molecule is not empty, i.e. has atoms.
    // we want a top-down view on it, i.e. the molecule should fit as well as
    // possible in the (X,Y)-plane. Equivalently, we want the Z axis to be parallel
    // to the normal vector of the molecule's fitting plane.
    // Thus we construct a suitable base-change rotation.
    Matrix3d rotation;
    rotation.row(2) = d->parent->normalVector();
    rotation.row(0) = rotation.row(2).unitOrthogonal();
    rotation.row(1) = rotation.row(2).cross(rotation.row(0));

    // set the camera's matrix to be (the 4x4 version of) this rotation.
    d->modelview.linear() = rotation;

    // now we want to move backwards, in order
    // to view the molecule from a distance, not from inside it.
    // This translation must be applied after the above rotation, so we
    // want a left-multiplication here. Whence pretranslate().
    pretranslate( - 3.0 * ( d->parent->radius() + CAMERA_NEAR_DISTANCE ) *
                  Vector3d::UnitZ() );

    // the above rotation is meant to be a rotation around the molecule's
    // center. So before this rotation is applied, the molecule's center
    // must be brought to the origin of the coordinate systemby a translation.
    // As this translation must be applied first, we want a right-multiplication here.
    // Whence translate().
    translate( - d->parent->center() );
  }

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

  return 0;
}

void CameraDirectLinearTransformation::decomposePMatrix(const Eigen::Matrix<double,3,4> &P)
{
    Matrix3d M = P.topLeftCorner<3,3>();
    Vector3d m3 = M.row(2).transpose();
    // Follow the HartleyZisserman - "Multiple view geometry in computer vision" implementation chapter 3

    Matrix3d P123,P023,P013,P012;
    P123 << P.col(1),P.col(2),P.col(3);
    P023 << P.col(0),P.col(2),P.col(3);
    P013 << P.col(0),P.col(1),P.col(3);
    P012 << P.col(0),P.col(1),P.col(2);

    double X = P123.determinant();
    double Y = -P023.determinant();
    double Z = P013.determinant();
    double T = -P012.determinant();
    C << X/T,Y/T,Z/T;

    // Image Principal points computed with homogeneous normalization
    this->principalPoint = (M*m3).eval().hnormalized().head<2>();

    // Principal vector  from the camera centre C through pp pointing out from the camera.  This may not be the same as  R(:,3)
    // if the principal point is not at the centre of the image, but it should be similar.
    this->principalVector  =  (M.determinant()*m3).normalized();
    this->R = this->K = Matrix3d::Identity();
    this->rq3(M,this->K,this->R);
    // To understand how K is formed http://ksimek.github.io/2013/08/13/intrinsic/
    // and also read http://ksimek.github.io/2012/08/14/decompose/
    K/=K(2,2); // EXTREMELY IMPORTANT TO MAKE THE K(2,2)==1 !!!

    // K = [ fx, s, x0;
    //       0, fy, y0;
    //       0,  0,  1];
    // Where fx is the focal length on x measured in pixels, fy is the focal length ony  measured in pixels

    // Negate the second column of K and R because the y window coordinates and camera y direction are opposite is positive
    // This is the solution I've found personally to correct the behaviour using OpenGL gluPerspective convention
    this->R.row(2)=-R.row(2);
    // Our 3x3 intrinsic camera matrix K needs two modifications before it's ready to use in OpenGL. First, for proper clipping, the (3,3) element of K must be -1. OpenGL's camera looks down the negative z-axis, so if K33 is positive, vertices in front of the camera will have a negative w coordinate after projection. In principle, this is okay, but because of how OpenGL performs clipping, all of these points will be clipped.
    //this->K.col(2) = -K.col(2);

    // t is the location of the world origin in camera coordinates.
    t = -R*C;
}

void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
	//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
	using namespace Eigen;

	Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
	Matrix3d EEt = E * E.transpose();
	Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
	Vector3d b1,b2;

#if 1
	//Method 1
	Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
	Vector3d bbt_diag = bbt.diagonal();
	if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
		b1 = bbt.row(0) / sqrt(bbt_diag(0));
		b2 = -b1;
	} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
		b1 = bbt.row(1) / sqrt(bbt_diag(1));
		b2 = -b1;
	} else {
		b1 = bbt.row(2) / sqrt(bbt_diag(2));
		b2 = -b1;
	}
#else
	//Method 2
	if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
		b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
		b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else {
		b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	}
#endif
	
	//Horn90 (19)
	Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
	cofactors.transposeInPlace();
	
	//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
	Matrix3d B1; B1 <<	0,-b1(2),b1(1),
						b1(2),0,-b1(0),
						-b1(1),b1(0),0;
	Matrix3d B2; B2 <<	0,-b2(2),b2(1),
						b2(2),0,-b2(0),
						-b2(1),b2(0),0;

	Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);

	//Horn90 (24)
	R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
	R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
	Map<Vector3d> t1(_t1),t2(_t2); 
	t1 = b1; t2 = b2;
	
	cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}

void Omega3::AcquisitionTask::run(){
    while(1){

        //! qDebug()<< "try to open the first available device";
        if (drdOpen () < 0) {
            //qDebug()<<"no device available...";
            //dhdSleep (2.0);
            omegaDetected = false;
        }
        else{
            omegaDetected = true;
        }

        if(omegaDetected){
            break;
        }

        sleep(1);
    }

    done  = 0;
    double p[DHD_MAX_DOF];
    double r[3][3];
    double v[DHD_MAX_DOF];
    double f[DHD_MAX_DOF];
    double normf, normt;

    double t0    = dhdGetTime ();
    double t1    = t0;
    bool   base  = false;
    bool   wrist = false;
    bool   grip  = false;

    //! Eigen objects (mapped to the arrays above)
    Map<Vector3d> position(&p[0], 3);
    Map<Vector3d> force   (&f[0], 3);
    Map<Vector3d> torque  (&f[3], 3);
    Map<Vector3d> velpos  (&v[0], 3);
    Map<Vector3d> velrot  (&v[3], 3);
    Matrix3d center;
    Matrix3d rotation;

    //! center of workspace
    center.setIdentity ();                           // rotation (matrix)
    double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0,  // translation
                                     0.0, 0.0, 0.0,  // rotation (joint angles)
                                     0.0 };          // gripper

    //! print out device identifier
    if (!drdIsSupported ()) {
      dhdSleep (2.0);
      drdClose ();
    }

    //! perform auto-initialization
    if (!drdIsInitialized () && drdAutoInit () < 0) {
      qDebug()<<"error: auto-initialization failed"<<dhdErrorGetLastStr ();
      dhdSleep (2.0);
    }
    else if (drdStart () < 0) {
      printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
      dhdSleep (2.0);
    }

    //! move to center
    drdMoveTo (nullPose);

    // request a null force (only gravity compensation will be applied)
    // this will only apply to unregulated axis
    for (int i=0; i<DHD_MAX_DOF; i++) f[i] = 0.0;
    drdSetForceAndTorqueAndGripperForce (f);

    // disable all axis regulation (but leave regulation thread running)
    drdRegulatePos  (base);
    drdRegulateRot  (wrist);
    drdRegulateGrip (grip);

    //! save current pos
    drdGetPositionAndOrientation (p, r);
    for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);

    this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);

    long long cpt = 0;

    // loop while the button is not pushed
    while (!done) {

      // synchronize with regulation thread
      drdWaitForTick ();

      // get position/orientation/gripper and update Eigen rotation matrix
      drdGetPositionAndOrientation (p, r);
      for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);

      if(cpt%31==0){
          this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
      }
      else{
          cpt += 1;
//.........这里部分代码省略.........

RigidBodySystem::StateVector<double> RigidBodySystem::dynamics(
    const double& t, const RigidBodySystem::StateVector<double>& x,
    const RigidBodySystem::InputVector<double>& u) const {
  using namespace std;
  using namespace Eigen;
  eigen_aligned_unordered_map<const RigidBody*, Matrix<double, 6, 1> > f_ext;

  // todo: make kinematics cache once and re-use it (but have to make one per
  // type)
  auto nq = tree->num_positions;
  auto nv = tree->num_velocities;
  auto num_actuators = tree->actuators.size();
  auto q = x.topRows(nq);
  auto v = x.bottomRows(nv);
  auto kinsol = tree->doKinematics(q, v);

  // todo: preallocate the optimization problem and constraints, and simply
  // update them then solve on each function eval.
  // happily, this clunkier version seems fast enough for now
  // the optimization framework should support this (though it has not been
  // tested thoroughly yet)
  OptimizationProblem prog;
  auto const& vdot = prog.AddContinuousVariables(nv, "vdot");

  auto H = tree->massMatrix(kinsol);
  Eigen::MatrixXd H_and_neg_JT = H;

  VectorXd C = tree->dynamicsBiasTerm(kinsol, f_ext);
  if (num_actuators > 0) C -= tree->B * u.topRows(num_actuators);

  // loop through rigid body force elements
  {
    // todo: distinguish between AppliedForce and ConstraintForce

    size_t u_index = 0;
    for (auto const& f : force_elements) {
      size_t num_inputs = f->getNumInputs();
      VectorXd force_input(u.middleRows(u_index, num_inputs));
      C -= f->output(t, force_input, kinsol);
      u_index += num_inputs;
    }
  }

  // apply joint limit forces
  {
    for (auto const& b : tree->bodies) {
      if (!b->hasParent()) continue;
      auto const& joint = b->getJoint();
      if (joint.getNumPositions() == 1 &&
          joint.getNumVelocities() ==
              1) {  // taking advantage of only single-axis joints having joint
                    // limits makes things easier/faster here
        double qmin = joint.getJointLimitMin()(0),
               qmax = joint.getJointLimitMax()(0);
        // tau = k*(qlimit-q) - b(qdot)
        if (q(b->position_num_start) < qmin)
          C(b->velocity_num_start) -=
              penetration_stiffness * (qmin - q(b->position_num_start)) -
              penetration_damping * v(b->velocity_num_start);
        else if (q(b->position_num_start) > qmax)
          C(b->velocity_num_start) -=
              penetration_stiffness * (qmax - q(b->position_num_start)) -
              penetration_damping * v(b->velocity_num_start);
      }
    }
  }

  // apply contact forces
  {
    VectorXd phi;
    Matrix3Xd normal, xA, xB;
    vector<int> bodyA_idx, bodyB_idx;
    if (use_multi_contact)
      tree->potentialCollisions(kinsol, phi, normal, xA, xB, bodyA_idx,
                                bodyB_idx);
    else
      tree->collisionDetect(kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx);

    for (int i = 0; i < phi.rows(); i++) {
      if (phi(i) < 0.0) {  // then i have contact
        // todo: move this entire block to a shared an updated "contactJacobian"
        // method in RigidBodyTree
        auto JA = tree->transformPointsJacobian(kinsol, xA.col(i), bodyA_idx[i],
                                                0, false);
        auto JB = tree->transformPointsJacobian(kinsol, xB.col(i), bodyB_idx[i],
                                                0, false);
        Vector3d this_normal = normal.col(i);

        // compute the surface tangent basis
        Vector3d tangent1;
        if (1.0 - this_normal(2) < EPSILON) {  // handle the unit-normal case
                                               // (since it's unit length, just
                                               // check z)
          tangent1 << 1.0, 0.0, 0.0;
        } else if (1 + this_normal(2) < EPSILON) {
          tangent1 << -1.0, 0.0, 0.0;  // same for the reflected case
        } else {                       // now the general case
          tangent1 << this_normal(1), -this_normal(0), 0.0;
          tangent1 /= sqrt(this_normal(1) * this_normal(1) +
                           this_normal(0) * this_normal(0));
//.........这里部分代码省略.........

int main(int argc, char** argv) {
    MatrixXf m(3,3);
    m << 1,2,3,4,5,6,7,8,9;
    Vector3f v =  m.col(0);
    Vector3f v2 =  m.col(1);
    Vector3f v3 =  m.col(2);

    cout<<"Mat = "<<m<<endl;
    cout<<"vector = " << v<<endl;

    MatrixXf n = (m.rowwise() -= v);
    cout<<"Mat2 = "<<n<<endl;

    Matrix3f r;
    r.col(0)  = v;
    r.col(1)  = v2;
    r.col(2)  = v3;
    cout<<"Mat3 = "<<r<<endl;
    r *= 2;
    cout<<"Mat3 = "<<r<<endl;
    cout<<"Mat3 = "<<m<<endl;


    float arr[] = {1,2,3};
    vector<float> w(arr, arr + sizeof(arr) / sizeof(float));
    for(int i = 0; i < w.size(); i+=1)
        cout<<w[i]<<"\t";
    cout<<endl<<"---------------"<<endl;

    /** Inverting a singular matrix. **/
    m << 1,0,0,1,0,0,1,0,0;
    cout<<" Should segfault/ get garbage: "<<m.determinant()<<endl;

    /** Affine matrix in Eigen. */
    MatrixXf ml(4,4);
    ml << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
    Affine3f t;
    Vector3f d(10,20,30);
    t.linear() = ml.transpose().block(0,0,3,3);
    t.translation() = ml.block(0,3,3,1);
    cout<<"Affine: "<<endl<<t.affine()<<endl;
    Vector3f tt(1,2,3);
    t.translation() += tt;
    cout<<"Affine after translation: "<<endl<<t.affine()<<endl;


    /** Blocks. */
    MatrixXf matr(3,2);
    matr << 1,2,3,4,5,6;
    MatrixXf combo(4,2);
    combo <<matr, MatrixXf::Ones(1,matr.cols());
    cout<<"Matr = "<<matr<<endl;
    cout<<"Comb = "<<combo<<endl;

    MatrixXf rrr = combo.block(0,0,3,2);
    cout<<"rrr = "<<rrr<<endl;


    /** Filling up a matrix*/
    std::cout<<"---------------------------------------"<<std::endl;
    MatrixXf matF(5,3);
    Vector3f vF(1,.3,3);
    Vector3f vF1(.123,.2,.3);
    Vector3f vF2(33.4,23.3,12.07);
    Vector3f vF3(0.54,8.96,14.3);
    Vector3f vF4(8.9,0.34,32.2);

    matF.row(0) = vF;
    matF.row(1) = vF1;
    matF.row(2) = vF2;
    matF.row(3) = vF3;
    matF.row(4) = vF4;

    RowVector3f means = matF.colwise().sum()/5;
    MatrixXf centered = (matF.rowwise() - means);

    cout<<"Matrix Fill = \n"<<matF<<std::endl;
    cout<<"Means = \n"<<means<<std::endl;
    cout<<"Centered = \n"<<centered<<std::endl;

    Eigen::JacobiSVD<MatrixXf> svd(centered / sqrt(5), ComputeFullV);
    cout << "Its singular values are:" << endl << svd.singularValues() << endl;
    cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
    MatrixXf V   = svd.matrixV();
    Vector3f f1  = V.col(0);
    Vector3f  f2 = V.col(1);
    Vector3f  f3 = V.col(2);
    cout << "f1(0)" << f1(0) << endl;

    VectorXf faa(V.rows());
    faa.setZero();
    //for (int i= 0; i < V.rows(); i++)
    V.col(2) = faa;
    cout << "V " << V<< endl;


    cout << "<v1,v2>" << f1.dot(f2) << endl;
    cout << "<v1,v3>" << f1.dot(f3) << endl;
    cout << "<v3,v2>" << f3.dot(f2) << endl;

//.........这里部分代码省略.........