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

Matrix3d computeTform(
    const Matrix2Xd& pointsFrom, const Matrix2Xd& pointsTo,
    const VectorXd& indices ) {
  Matrix3d normMatFrom;
  Matrix3d normMatTo;
  Matrix3d T = computeSimilarity(
    normalizePoints( pointsFrom, indices, normMatFrom ),
    normalizePoints( pointsTo, indices, normMatTo ) );
  return normMatFrom.transpose() * ( T * normMatTo.transpose().inverse() );
}

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

boost::array<pair<Vector3d,Matrix3d >, 2 > PlaneFromConic(const Conic& c, double plane_circle_radius, const Matrix3d& K )
{
  const Matrix3d Cn = K.transpose() * c.C *K;
  Eigen::JacobiSVD<Matrix3d> svd(Cn, ComputeFullU );
  const Matrix3d R1 = svd.matrixU();
  const Vector3d l = svd.singularValues();
  const double t = atan( sqrt( (l[1]-l[0]) / (l[2]-l[1]) ) );

  boost::array<pair<Vector3d,Matrix3d >, 2> r;

  for( int i=0; i < 2; ++i )
  {
    const double theta = (i*2-1)*t;
    const Matrix3d R2 = RotY(theta);
    const Matrix3d R = R1 * R2;
//    const Matrix3d C__ = R.T() * Cn * R;
    const Vector3d n = R * Vector3d(0,0,-1);
    const double d = sqrt( (l[1]*l[1])/(l[0]*l[2]) ) * plane_circle_radius;

    r[i].first = n.normalized() / d;
    r[i].second = R;
  }

  return r;
}

Vector2d MSCKF::projectPoint(Vector3d feature_pose, Matrix3d R_gb, Vector3d p_gb, Vector3d p_cb)
{
    Vector2d zij;
    zij = cam.h(R_cb * R_gb.transpose() * (feature_pose - p_gb) + p_cb);
    
    return zij;
}

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

    void BodyNode::updateTransform() {
        mT = mJointParent->getLocalTransform();
        if (mNodeParent) mW = mNodeParent->mW * mT;
        else mW = mT;

        // update the inertia matrix 
        Matrix3d R = mW.topLeftCorner(3,3);
        if(mVizShape!=NULL)
            mIc = R*mI*R.transpose();
    }

 RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
     //mb=ma
     //hb=R*h
     //Ib = R(Ia)R' with r=0
     Matrix3d R = Map<Matrix3d>(M.data);
     RotationalInertia Ib;
     Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
     
     return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
 }

double angle_mismatch(const Matrix3d& Q, const Matrix3d& R)
{
  //std::cout << "Q\n" << Q << "\nR\n" << R << std::endl;
  Matrix3d S = Q.transpose()*R;
  //std::cout << "S\n: " << S << std::endl;
  double thecos = (S.trace()-1.0)/2.0;
  thecos = std::max( std::min ( thecos, 1.0), -1.0);
  //std::cout << "thecos: " << thecos << " leads to angle: " << acos(thecos) << " * " << ( S(1,2) < 0.0 ? -1.0 : 1.0) << std::endl;

  return (acos(thecos) * ( S(1,2) < 0.0 ? -1.0 : 1.0));
}

const Matrix3d VectorMath::TMatrix(const Vector3d &v)
{
    double vnormsq = v.dot(v);
    Matrix3d I;
    I.setIdentity();
    if(vnormsq == 0)
        return I;

    Matrix3d R = rotationMatrix(v);
    return (v*v.transpose() + (R.transpose()-I)*crossProductMatrix(v))/vnormsq;
}

double Cost( const pair<Vector3d,Matrix3d >& plane, const Conic& conic, const Matrix3d& K )
{
  const Matrix3d Cn = K.transpose() * conic.C *K;
  const Matrix3d& R = plane.second;
  const Matrix3d C_ = R.transpose() * Cn * R;
  const double a = C_(0,0);
//  const double b = C_[0][1] / 2.0;
  const double c = C_(1,1);
  const double amc = abs(a - c);
  return amc / max(abs(a),abs(c));;// + b*b;
}

void force_feedback_pos_rate_control(void){
  
  // ****************************************************************************************************************
  // Force feedback when in rate control area
  // ****************************************************************************************************************
  
  // Variables
  Ks = MatrixXd::Zero(3,3);
  Ks(0,0) = scale[0];
  Ks(1,1) = scale[1];
  Ks(2,2) = scale[2];
 
  Eigen::Vector3d omni_force = Eigen::Vector3d::Zero();
  double R = 0.05;
  double Fmax = 3.3; // 3.3 N es la máxima força nominal
  double Kf = 60;
  double Kv = 0.5;

  // Displacement
  dm = pm_i - pm_0;
  ds = Ks * Rs.transpose() * Rm * dm;  
  
  // Omni feedback force computation
  double D = sqrt( ds(0)*ds(0) + ds(1)*ds(1) );  
  
  if (D > R)	omni_force = - Kf * (D - R) * (ds/D);
  omni_force += - Kv*vm_i; // Damping
  omni_force(2) = 0.0;     // No force in vertical axis
  omni_force = Rm.transpose() * Rs * omni_force; // To master reference frame
  
  if (omni_force(0) < -Fmax) omni_force(0) = -Fmax;
  if (omni_force(0) > Fmax)  omni_force(0) = Fmax;
  if (omni_force(1) < -Fmax) omni_force(1) = -Fmax;
  if (omni_force(1) > Fmax)  omni_force(1) = Fmax;  

  // Publish
  force.x = omni_force(0);
  force.y = omni_force(1);
  force.z = omni_force(2);
  pub_OmniForceFeedback.publish(force);
}

    void Edge_V_V_GICPLandmark::linearizeOplus()
    {
      //  std::cout << "START Edge_V_V_GICPLandmark::linearizeOplus() " << std::endl;
      VertexPointXYZ* vp0 = static_cast<VertexPointXYZ*>(_vertices[0]);
      VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]);
      Vector3d p1 = measurement().pos1;

      if (!vp0->fixed())
        {
          _jacobianOplusXi.block<3,3>(0,0) = -Matrix3d::Identity();
        }

      if (!vp1->fixed())
        {
          Matrix3d R1 = vp1->estimate().matrix().topLeftCorner<3,3>();
          _jacobianOplusXj.block<3,3>(0,0) = R1;
          _jacobianOplusXj.block<3,1>(0,3) = R1*dRidx.transpose()*p1;
          _jacobianOplusXj.block<3,1>(0,4) = R1*dRidy.transpose()*p1;
          _jacobianOplusXj.block<3,1>(0,5) = R1*dRidz.transpose()*p1;
        }
    }

void Camera::updateOthers() {
    P_ /= P_.row(2).head<3>().squaredNorm();

	// RQ Factorization
    Matrix3d M = P_.topLeftCorner<3, 3>();
	Matrix3d reverseRows;
	reverseRows << 0, 0, 1, 0, 1, 0, 1, 0, 0;

    const Eigen::Matrix3d qrMatrix = (reverseRows * M).transpose();
    const Eigen::HouseholderQR<Matrix3d> qrDecomp(qrMatrix);
    const Matrix3d matrixQ = qrDecomp.householderQ();
    const Matrix3d matrixR = qrDecomp.matrixQR().triangularView<Upper>();

	R_ = reverseRows * matrixQ.transpose();
	K_ = reverseRows * matrixR.transpose() * reverseRows;

	// Eigen doesn't require diagonal elements of R (in the QR
	// decomposition) to be positive, so correct for this since the
	// intrinsic matrix should have positive diagonal elements
	for(int axis = 2; axis >= 0; --axis) {
		if(K_(axis, axis) < 0) {
			K_(axis, axis) = -(K_(axis, axis));
			R_.row(axis) = -(R_.row(axis));
		}

		if(K_(axis, 2) < 0)
			K_(axis, 2) = -K_(axis, 2);
	}

	orthonormalize(R_);

	Kinv_ = K_.inverse();
	Rinv_ = R_.transpose();
	t_ = Kinv_ * P_.col(3);
	C_ = -Rinv_ * t_;

	updatePrincipleRay();
}

void localizerGVG::propagate(double Vm, double Wm, double dt) {  
  Matrix3d Fr;
  Fr<<1, 0, -Vm*dt*sin(oldYaw),
      0, 1,  Vm*dt*cos(oldYaw),
      0, 0,  1;
  MatrixXd Gr(3,2);
  Gr<< -dt*cos(oldYaw), 0, 
       -dt*sin(oldYaw), 0,
        0,             -dt;
    
  P.block(0,0,3,3)=Fr*P.block(0,0,3,3)*Fr.transpose()+Gr*Qr*Gr.transpose();
  CF=Fr*CF;
  normalizeCovariance();
}

Matrix3d skewlog(Matrix3d M){
    Matrix3d skew;
    double val = (M.trace() - 1.f)/2.f;
    if(val > 1.f)
        val = 1.f;
    else if (val < -1.f)
        val = -1.f;
    double theta = acos(val);
    if(theta == 0.f)
        skew << 0,0,0,0,0,0,0,0,0;
    else
        skew << (M-M.transpose())/(2.f*sin(theta))*theta;
    return skew;
}