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

bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
    if (corres.size() >= 15)
    {
        vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }
        cv::Mat mask;
        cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
        cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
        cv::Mat rot, trans;
        int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
        //cout << "inlier_cnt " << inlier_cnt << endl;

        Eigen::Matrix3d R;
        Eigen::Vector3d T;
        for (int i = 0; i < 3; i++)
        {   
            T(i) = trans.at<double>(i, 0);
            for (int j = 0; j < 3; j++)
                R(i, j) = rot.at<double>(i, j);
        }

        Rotation = R.transpose();
        Translation = -R.transpose() * T;
        if(inlier_cnt > 12)
            return true;
        else
            return false;
    }
    return false;
}

double GreenStrain_LIMSolver3D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
  // green strain energy
  double shape = 0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
  for(int t=0;t<mesh->Tetrahedra->rows();t++)
  {
    Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
    Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
    Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
    Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);

    Eigen::Matrix<double,3,4> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;
    V.col(3) = D;

    Eigen::Matrix3d F = V*Ms.block<4,3>(0,3*t);
    Eigen::Matrix3d E = (F.transpose()*F - I);
    shape += E.squaredNorm()*Divider;
  }

  return shape;
}

bool CGEOM::generate_scene_trans
( const SceneGeneratorOptions& sc_opts,
  Eigen::MatrixXd& mP3D,
  Eigen::MatrixXd& mMeasT,
  Eigen::MatrixXd& mMeasN,
  Eigen::Matrix3d& mR,
  Eigen::Vector3d& mt
) {
    if( !generate_scene( sc_opts,
                         mP3D, mMeasT, mMeasN
                       ) ) {
        return false;
    }

    // Create random transform
    mt = mP3D.rowwise().mean();
    const double drotx = rand_range_d( -45., 45. )*3.14159/180.;
    const double droty = rand_range_d( -45., 45. )*3.14159/180.;
    const double drotz = rand_range_d( -45., 45. )*3.14159/180.;
#if 1
    mR =
        ( CEIGEN::skew_rot<Eigen::Matrix3d,double>( drotx, 0., 0. ) +
          CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., droty , 0.) +
          CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., 0., drotz ) ).exp();
#else
    mR = Eigen::Matrix3d::Identity();
#endif

    mP3D.colwise() -= mt;
    mP3D = mR.transpose() * mP3D;

    return true;
}

  // Assume t = double[3], q = double[4]
  void EstimateTfSVD(double* t, double* q)
  {
    // Assemble the correlation matrix H = target * reference'
    Eigen::Matrix3d H = (cloud_tgt_demean * cloud_ref_demean.transpose ()).topLeftCorner<3, 3>();

    // Compute the Singular Value Decomposition
    Eigen::JacobiSVD<Eigen::Matrix3d> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d u = svd.matrixU ();
    Eigen::Matrix3d v = svd.matrixV ();

    // Compute R = V * U'
    if (u.determinant () * v.determinant () < 0)
      {
	for (int i = 0; i < 3; ++i)
	  v (i, 2) *= -1;
      }

    //    std::cout<< "centroid_src: "<<centroid_src(0) <<" "<< centroid_src(1) <<" "<< centroid_src(2) << " "<< centroid_src(3)<<std::endl;
    //    std::cout<< "centroid_tgt: "<<centroid_tgt(0) <<" "<< centroid_tgt(1) <<" "<< centroid_tgt(2) << " "<< centroid_tgt(3)<<std::endl;
    
    Eigen::Matrix3d R = v * u.transpose ();

    const Eigen::Vector3d Rc (R * centroid_tgt.head<3> ());
    Eigen::Vector3d T = centroid_ref.head<3> () - Rc;

    // Make sure these memory locations are valid
    assert(t != NULL && q!=NULL);
    Eigen::Quaterniond Q(R);
    t[0] = T(0);  t[1] = T(1);  t[2] = T(2);
    q[0] = Q.w(); q[1] = Q.x(); q[2] = Q.y(); q[3] = Q.z();
  }

//==============================================================================
// Note: Taken from Springer Handbook, chapter 2.2.11
void Inertia::computeSpatialTensor()
{
  Eigen::Matrix3d C = math::makeSkewSymmetric(mCenterOfMass);

  // Top left
  mSpatialTensor.block<3,3>(0,0) = getMoment() + mMass*C*C.transpose();

  // Bottom left
  mSpatialTensor.block<3,3>(3,0) = mMass*C.transpose();

  // Top right
  mSpatialTensor.block<3,3>(0,3) = mMass*C;

  // Bottom right
  mSpatialTensor.block<3,3>(3,3) = mMass*Eigen::Matrix3d::Identity();
}

Eigen::Matrix3d LinearAlgebra::transposeMatrixM(const Eigen::Matrix3d& M) {
	Eigen::Matrix3d result;

	// TODO: return the transpose of matrix M
	result = M.transpose();

	return result;
}

//==============================================================================
Eigen::Vector3d BallJoint::getPositionDifferencesStatic(
    const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const
{
  const Eigen::Matrix3d R1 = convertToRotation(_q1);
  const Eigen::Matrix3d R2 = convertToRotation(_q2);

  return convertToPositions(R1.transpose() * R2);
}

void fundamental2essential( Eigen::Matrix3d &Fundamental, Eigen::Matrix3d &kalibration,
		        Eigen::Matrix3d &Essential)
{
    Essential = kalibration.transpose()*(Fundamental*kalibration);
    Essential = Essential/Essential(2,2);
    
    return;
}

const Eigen::Matrix3d CMiniVisionToolbox::getFundamental( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo, const Eigen::Matrix3d& p_matIntrinsic )
{
    //ds get essential matrix
    const Eigen::Matrix3d matEssential( CMiniVisionToolbox::getEssential( p_matTransformationFrom, p_matTransformationTo ) );

    //ds compute fundamental matrix: http://en.wikipedia.org/wiki/Fundamental_matrix_%28computer_vision%29
    const Eigen::Matrix3d matIntrinsicInverse( p_matIntrinsic.inverse( ) );
    return matIntrinsicInverse.transpose( )*matEssential*matIntrinsicInverse;
}

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    if (U.determinant() * V.determinant() < 0)
	{
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
	}
    
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
    type = "Oriented";
    orientedPoints.clear();
    
    // compute mean
    Eigen::Vector3d center;
    center.setZero();
    for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
        center += v->position;
    }
    center /= (double)vertices.size();
    
    // adjust for mean and compute covariance
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        Eigen::Vector3d pAdg = v->position - center;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)vertices.size();

    // compute eigenvectors for the covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();

    // project min and max points on each principal axis
    double min1 = INFINITY, max1 = -INFINITY;
    double min2 = INFINITY, max2 = -INFINITY;
    double min3 = INFINITY, max3 = -INFINITY;
    double d = 0.0;
    eigenVectors.transpose();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        d = eigenVectors.row(0).dot(v->position);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = eigenVectors.row(1).dot(v->position);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = eigenVectors.row(2).dot(v->position);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // add points to vector
    orientedPoints.push_back(eigenVectors.row(0) * min1);
    orientedPoints.push_back(eigenVectors.row(0) * max1);
    orientedPoints.push_back(eigenVectors.row(1) * min2);
    orientedPoints.push_back(eigenVectors.row(1) * max2);
    orientedPoints.push_back(eigenVectors.row(2) * min3);
    orientedPoints.push_back(eigenVectors.row(2) * max3);
}

ELASStereo::ELASStereo(calibu::CameraRig& rig, const unsigned int width,
                       const unsigned int height)
  : m_dDisparity(width, height), m_dDepth(width, height) {
  if (rig.cameras.size() != 2) {
    std::cerr << "Two camera models are required to run this program!"
              << std::endl;
    exit(1);
  }

  m_width = width;
  m_height = height;

  Eigen::Matrix3f CamModel0 = rig.cameras[0].camera.K().cast<float>();
  Eigen::Matrix3f CamModel1 = rig.cameras[1].camera.K().cast<float>();

  roo::ImageIntrinsics camMod[] = {
    {CamModel0(0, 0), CamModel0(1, 1), CamModel0(0, 2), CamModel0(1, 2)},
    {CamModel1(0, 0), CamModel1(1, 1), CamModel1(0, 2), CamModel1(1, 2)}};

  m_Kl = camMod[0][0].Matrix();

  // print selected camera model
  std::cout << "Camera Model used: " << std::endl << camMod[0][0].Matrix()
      << std::endl;

  Eigen::Matrix3d RDFvision;
  RDFvision << 1, 0, 0, 0, 1, 0, 0, 0, 1;
  Eigen::Matrix3d RDFrobot;
  RDFrobot << 0, 1, 0, 0, 0, 1, 1, 0, 0;
  Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity();
  T_vis_ro.block<3, 3>(0, 0) = RDFvision.transpose() * RDFrobot;
  Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity();
  T_ro_vis.block<3, 3>(0, 0) = RDFrobot.transpose() * RDFvision;

  const Sophus::SE3d T_rl =
      T_rlFromCamModelRDF(rig.cameras[0], rig.cameras[1], RDFvision);
  m_baseline = T_rl.translation().norm();

  std::cout << "Baseline is: " << m_baseline << std::endl;
}

void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions)
{
    // compute mean
    Eigen::Vector3d cm;
    cm.setZero();
    for (size_t i = 0; i < positions.size(); i++) {
        cm += positions[i];
    }
    cm /= (double)positions.size();
    
    // adjust for mean and compute covariance matrix
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (size_t i = 0; i < positions.size(); i++) {
        Eigen::Vector3d pAdg = positions[i] - cm;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)positions.size();
    
    // compute eigenvectors for covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
    
    // set axes
    eigenVectors.transpose();
    xAxis = eigenVectors.row(0);
    yAxis = eigenVectors.row(1);
    zAxis = eigenVectors.row(2);
    
    // project min and max points on each principal axis
    double min1 = INF, max1 = -INF;
    double min2 = INF, max2 = -INF;
    double min3 = INF, max3 = -INF;
    double d = 0.0;
    for (size_t i = 0; i < positions.size(); i++) {
        d = xAxis.dot(positions[i]);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = yAxis.dot(positions[i]);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = zAxis.dot(positions[i]);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // set center and halflengths
    center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2;
    halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2;
}

void poseJacobian(const Eigen::Matrix3d& rotation, Eigen::Matrix<double, 6, 6>& jac, const double rot_angle_threshold)
{
	Eigen::Matrix3d i3 = Eigen::Matrix3d::Identity();

	// convert rotation matrix into axis-angle representation
	double rot_angle;
	Eigen::Vector3d rot_axis;
	getAngleAxis(rotation.transpose(), rot_angle, rot_axis);

	// create the rotation jacobian
	Eigen::Matrix3d L_theta_u;

	double sinc_part;
	sinc_part = sva::sinc(rot_angle)/std::pow(sva::sinc(rot_angle/2.), 2);

	Eigen::Matrix3d axis_antisym;
	getSkewSym(rot_axis, axis_antisym);

	L_theta_u = i3 - rot_angle*0.5*axis_antisym + (1-(sinc_part))*axis_antisym*axis_antisym;
	jac << L_theta_u, Eigen::Matrix3d::Zero(),
				 Eigen::Matrix3d::Zero(), rotation.transpose();
}

IMPALGEBRA_BEGIN_NAMESPACE

Eigen::Matrix3d get_covariance(const Gaussian3D &g) {
  Transformation3D trans = g.get_reference_frame().get_transformation_to();
  Vector3D center = trans.get_translation();
  Vector4D iq = trans.get_rotation().get_quaternion();
  Eigen::Quaterniond q(iq[0], iq[1], iq[2], iq[3]);
  Eigen::Matrix3d rot = q.toRotationMatrix();
  Vector3D variances = g.get_variances();
  Eigen::Matrix3d rad = Eigen::Vector3d(variances[0], variances[1],
                                                variances[2]).asDiagonal();
  Eigen::Matrix3d covar = rot * (rad * rot.transpose());
  return covar;
}