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

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

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 tangent_and_bitangent(const Eigen::Vector3d & n_,
    Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
  double rmin = 0.99;
  double n0 = n_(0), n1 = n_(1), n2 = n_(2);
  if (std::abs(n0) <= rmin) {
    rmin = std::abs(n0);
    t_(0) = 0.0;
    t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
    t_(2) =   n1 / std::sqrt(1.0 - std::pow(n0, 2));
  }
  if (std::abs(n1) <= rmin) {
    rmin = std::abs(n1);
    t_(0) =   n2 / std::sqrt(1.0 - std::pow(n1, 2));
    t_(1) =   0.0;
    t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
  }
  if (std::abs(n2) <= rmin) {
    rmin = std::abs(n2);
    t_(0) =  n1 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(2) =  0.0;
  }
  b_ = n_.cross(t_);
  // Check that the calculated Frenet-Serret frame is left-handed (levogiro)
  // by checking that the determinant of the matrix whose columns are the normal,
  // tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
  Eigen::Matrix3d M;
  M.col(0) = n_;
  M.col(1) = t_;
  M.col(2) = b_;
  if (boost::math::sign(M.determinant()) != 1) {
    PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
  }
}

Gaussian3D get_gaussian_from_covariance(const Eigen::Matrix3d &covar,
                                        const Vector3D &center) {
  Rotation3D rot;
  Vector3D radii;

  // get eigen decomposition and sort by eigen vector
  Eigen::EigenSolver<Eigen::Matrix3d> es(covar);
  Eigen::Matrix3d evecs = es.eigenvectors().real();
  Eigen::Vector3d evals = es.eigenvalues().real();

  // fill in sorted stuff
  for (int i = 0; i < 3; i++) {
    radii[i] = evals[i];
  }

  // reflect if necessary
  double det = evecs.determinant();
  // std::cout<<"Determinant is "<<det<<std::endl;
  if (det < 0) {
    Eigen::Matrix3d reflect = Eigen::Vector3d(1, 1, -1).asDiagonal();
    evecs = evecs * reflect;
  }

  // create rotation matrix and return
  Eigen::Quaterniond eq(evecs);
  rot = Rotation3D(eq.w(), eq.x(), eq.y(), eq.z());
  return Gaussian3D(ReferenceFrame3D(Transformation3D(rot, center)), radii);
}

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

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

	return result;
}

bool checkCoherentRotation(Eigen::Matrix3d &Rot)
{
    double det1 = Rot.determinant();
    if(fabsf( det1 )-1.0 > 1e-07)
    {
        std::cerr << "det(R) != +-1.0, this is not a rotation matrix" << '\n';
        return false;
    }
    return true;
}

void MarkerArrayVisualizer::drawGaussianDistributions(const char* ns, const std::vector<Eigen::Vector3d>& mu, const std::vector<Eigen::Matrix3d>& sigma, double probability, const std::vector<double>& offset)
{
    visualization_msgs::MarkerArray marker_array;

    const int size = mu.size();

    for (int i=0; i<size; i++)
    {
        visualization_msgs::Marker marker;

        marker.header.frame_id = "/world";
        marker.header.stamp = ros::Time::now();

        marker.ns = ns;
        marker.id = i;
        marker.type = visualization_msgs::Marker::SPHERE;
        marker.action = visualization_msgs::Marker::ADD;

        // axis: eigenvectors
        // radius: eigenvalues
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(sigma[i], Eigen::ComputeThinU | Eigen::ComputeThinV);
        const Eigen::VectorXd& r = svd.singularValues();
        Eigen::Matrix3d Q = svd.matrixU();

        // to make determinant 1
        if (Q.determinant() < 0)
            Q.col(2) *= -1.;
        const Eigen::Quaterniond q(Q);

        marker.pose.position.x = mu[i](0);
        marker.pose.position.y = mu[i](1);
        marker.pose.position.z = mu[i](2);
        marker.pose.orientation.x = q.x();
        marker.pose.orientation.y = q.y();
        marker.pose.orientation.z = q.z();
        marker.pose.orientation.w = q.w();

        const double probability_radius = gaussianDistributionRadius3D(probability);

        marker.scale.x = 2. * (probability_radius * std::sqrt(r[0]) + offset[i]);
        marker.scale.y = 2. * (probability_radius * std::sqrt(r[1]) + offset[i]);
        marker.scale.z = 2. * (probability_radius * std::sqrt(r[2]) + offset[i]);

        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.color.a = 0.25;

        marker.lifetime = ros::Duration();

        marker_array.markers.push_back(marker);
    }

    publish(marker_array);
}

TEST(OdometryCalibration, Calibration) {
	FileIO fileIO;
	std::string path = ros::package::getPath("odometry_calibration") + "/data/calib.dat";
	try {
		fileIO.loadFromFile(path.c_str());
	} catch(const std::runtime_error& e) {
		ASSERT_TRUE(false) << "Could not load calibration data file";
	}
	Eigen::Matrix3d calibrationMatrix = OdometryCalibration::calibrateOdometry(fileIO.measurements);
	ASSERT_NEAR(-0.0754092, calibrationMatrix.determinant(), 1e-5);
}

void MarkerArrayVisualizer::drawEllipsoids(const char* ns, const std::vector<Eigen::Vector3d>& center, const std::vector<Eigen::Matrix3d>& A)
{
    visualization_msgs::MarkerArray marker_array;

    const int size = center.size();

    for (int i=0; i<size; i++)
    {
        visualization_msgs::Marker marker;

        marker.header.frame_id = "/world";
        marker.header.stamp = ros::Time::now();

        marker.ns = ns;
        marker.id = i;
        marker.type = visualization_msgs::Marker::SPHERE;
        marker.action = visualization_msgs::Marker::ADD;

        // axis: eigenvectors
        // radius: eigenvalues
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A[i], Eigen::ComputeThinU | Eigen::ComputeThinV);
        const Eigen::VectorXd& r = svd.singularValues();
        Eigen::Matrix3d Q = svd.matrixU();

        // to make determinant 1
        if (Q.determinant() < 0)
            Q.col(2) *= -1.;
        const Eigen::Quaterniond q(Q);

        marker.pose.position.x = center[i](0);
        marker.pose.position.y = center[i](1);
        marker.pose.position.z = center[i](2);
        marker.pose.orientation.x = q.x();
        marker.pose.orientation.y = q.y();
        marker.pose.orientation.z = q.z();
        marker.pose.orientation.w = q.w();

        marker.scale.x = 2. * r(0);
        marker.scale.y = 2. * r(1);
        marker.scale.z = 2. * r(2);

        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.color.a = 0.2;

        marker.lifetime = ros::Duration();

        marker_array.markers.push_back(marker);
    }

    publish(marker_array);
}

bool determinantCorrection(Eigen::Matrix3d &Rot)
{
    double det1 = Rot.determinant();
    if( det1 < -0.9999999 && det1 > -1.0000001 )
    {
        // When a rotation matrix is a reflection because its determinant is -1
        Rot.col(2) = -1.0*Rot.col(2); // Corrects the third column sign
        std::cout << "Rotation matrix has det = -1, correct procedure to achieve det = 1\n";
        return true;
    }
    return false;
}

double PCGMMReg_func::weight_l2(PCObject &model, PCObject &scene)
{
    // reference :
    // Robust Point Set Registration Using Gaussian Mixture Models
    // Bing Jina, and Baba C. Vemuri
    // IEEE Transactions on Pattern Analysis and Machine Intelligence 2010

    double energy1 = 0.;
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            Eigen::Matrix3d cov = model.gmm.at(i).covariance + model.gmm.at(j).covariance;
            Eigen::Vector3d mean = model.gmm.at(i).mean - model.gmm.at(j).mean;
            Eigen::Matrix3d invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
            energy1 += model.gmm.at(i).weight*model.gmm.at(j).weight*gauss;
        }
    }
//    cout<<"m "<<m<<endl;
//    cout<<"s "<<s<<endl;
    double energy2 = 0.;
    for(int i=0;i<m;i++){
        double sum[3] = {0.,0.,0.};
        for(int j=0;j<s;j++){
            Eigen::Matrix3d cov = model.gmm.at(i).covariance + scene.gmm.at(j).covariance;
            Eigen::Vector3d mean = model.gmm.at(i).mean - scene.gmm.at(j).mean;
            Eigen::Matrix3d invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
            energy2 += model.gmm.at(i).weight*scene.gmm.at(j).weight*gauss;
//            cout<<"weight i "<<model.gmm.at(i).weight<<endl;
//            cout<<"weight j "<<scene.gmm.at(j).weight<<endl;
//            cout<<"a "<<a<<endl;
//            cout<<"gauss "<<gauss<<endl;


            // gradient [m,d]
            double derv_x = -0.5*(2*mean[0]*invij(0,0) + mean[1]*(invij(0,1)+invij(1,0)) + mean[2]*(invij(0,2)+invij(2,0)));
            double derv_y = -0.5*(mean[0]*(invij(1,0)+invij(0,1)) + 2*mean[1]*invij(1,1) + mean[2]*(invij(1,2)+invij(2,1)));
            double derv_z = -0.5*(mean[0]*(invij(2,0)+invij(0,2)) + mean[1]*(invij(2,1)+invij(1,2)) + 2*mean[2]*invij(2,2));

            sum[0] += scene.gmm.at(j).weight*gauss*derv_x;
            sum[1] += scene.gmm.at(j).weight*gauss*derv_y;
            sum[2] += scene.gmm.at(j).weight*gauss*derv_z;

        }
        gradient[i][0] = -2.*model.gmm.at(i).weight*sum[0];
        gradient[i][1] = -2.*model.gmm.at(i).weight*sum[1];
        gradient[i][2] = -2.*model.gmm.at(i).weight*sum[2];
    }
    double energy3 = 0.;
    for(int i=0;i<s;i++){
        for(int j=0;j<s;j++){
            Eigen::Matrix3d cov = scene.gmm.at(i).covariance + scene.gmm.at(j).covariance;
            Eigen::Vector3d mean = scene.gmm.at(i).mean - scene.gmm.at(j).mean;
            Eigen::Matrix3d invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
            energy3 += scene.gmm.at(i).weight*scene.gmm.at(j).weight*gauss;
        }
    }
    return energy1 - 2*energy2 + energy3;
//    cout<<"energy2 "<<energy2<<endl;
//    return -2*energy2;
}

double covToSDMetres(const Eigen::Matrix3d & S)
{
    return pow(S.determinant(), 1.0/6.0); //Approx. sigma in metres
}

void mesh_core::findSphereTouching4Points(
      Eigen::Vector3d& center,
      double& radius,
      const Eigen::Vector3d& a,
      const Eigen::Vector3d& b,
      const Eigen::Vector3d& c,
      const Eigen::Vector3d& d)
{
  Eigen::Matrix3d m;
  m.col(0) = b - a;
  m.col(1) = c - a;
  m.col(2) = d - a;

  double det = m.determinant();

  // points are coplanar?
  if (std::abs(det) <= std::numeric_limits<double>::epsilon())
  {
    findSphereTouching4PointsCoplanar(center, radius, a,b,c,d);
    if (g_verbose)
    {
      logInform("findSphereTouching4Points() COPLANAR  QQQQ");
      logInform("           a = (%7.3f %7.3f %7.3f)",
        a.x(),
        a.y(),
        a.z());
      logInform("           b = (%7.3f %7.3f %7.3f)",
        b.x(),
        b.y(),
        b.z());
      logInform("           c = (%7.3f %7.3f %7.3f)",
        c.x(),
        c.y(),
        c.z());
      logInform("           d = (%7.3f %7.3f %7.3f)",
        d.x(),
        d.y(),
        d.z());
      logInform("           s = (%7.3f %7.3f %7.3f) r=%7.3f",
        center.x(),
        center.y(),
        center.z(),
        radius);
    }
    return;
  }

  double ab_lensq = m.col(0).squaredNorm();
  double ac_lensq = m.col(1).squaredNorm();
  double ad_lensq = m.col(2).squaredNorm();
  Eigen::Vector3d rel_center = ((ad_lensq * m.col(0).cross(m.col(1))) +
                                (ac_lensq * m.col(2).cross(m.col(0))) +
                                (ab_lensq * m.col(1).cross(m.col(2)))) /
                               (2.0 * det);

  radius = rel_center.norm();
  center = rel_center + a;

  if (g_verbose)
  {
    logInform("findSphereTouching4Points()");
    logInform("           a = (%7.3f %7.3f %7.3f)",
      a.x(),
      a.y(),
      a.z());
    logInform("           b = (%7.3f %7.3f %7.3f)",
      b.x(),
      b.y(),
      b.z());
    logInform("           c = (%7.3f %7.3f %7.3f)",
      c.x(),
      c.y(),
      c.z());
    logInform("           d = (%7.3f %7.3f %7.3f)",
      d.x(),
      d.y(),
      d.z());
    logInform("           s = (%7.3f %7.3f %7.3f) r=%7.3f",
      center.x(),
      center.y(),
      center.z(),
      radius);
  }
}

/*
 * Compute the homography parameters
 */
void RSTEstimator::leastSquaresEstimate(std::vector<std::pair<Eigen::Vector3d,Eigen::Vector3d> *> &points, 
									    std::vector<double> &parameters)
{
	
	unsigned int N = points.size(); 
	
	//checks
	if (N<3) {
		std::cout << "At least 3 point correspondences are needed" << std::endl;
		return;
	}
	
	Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0);
	Eigen::MatrixXd model(3,N);
	Eigen::MatrixXd data(3,N);

	Eigen::MatrixXd Ryx;
	Eigen::Matrix3d R;
	Eigen::Vector3d T;
	
	
	//compute centroids
	for (int i=0 ; i<N; i++) {

		model.block(0,i,3,1) = points[i]->first;
		data.block(0,i,3,1) = points[i]->second;
		
		centroide_model += points[i]->first;
		centroide_data  += points[i]->second;
	}
		
	centroide_model = centroide_model/N; 
	centroide_data = centroide_data/N; 
		
	
	//Subtract centroids to data
	for (int i=0; i<N; i++){
		model.block(0,i,3,1) -= centroide_model;
		data.block(0,i,3,1) -= centroide_data;
	}
	
	Ryx = data * model.transpose(); //maps data into model
	
	JacobiSVD<Eigen::Matrix3d> svd(Ryx, ComputeFullU | ComputeFullV);
	
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();

  if (U.determinant()*V.determinant()<0) {
		for (int i=0; i<3; ++i) {
			V(i,2) *=-1;
		}
	}
	
	R = V * U.transpose();
	T = centroide_model - R*centroide_data;
			
	
	parameters.push_back(R(0,0));  //R11
	parameters.push_back(R(0,1));  //R12
	parameters.push_back(R(0,2));  //R13
	
	parameters.push_back(R(1,0));  //R21
	parameters.push_back(R(1,1));  //R22
	parameters.push_back(R(1,2));  //R23
	
	parameters.push_back(R(2,0));  //R31
	parameters.push_back(R(2,1));  //R32
	parameters.push_back(R(2,2));  //R33
	
	parameters.push_back(T(0));    //T1
	parameters.push_back(T(1));    //T2
	parameters.push_back(T(2));    //T3
	

	/*
		H = | Theta1   Theta2   Theta3   Theta10 |
				| Theta4   Theta5   Theta6   Theta11 |
				| Theta7   Theta8   Theta9   Theta12 |
				|   0         0        0        1	   |
	 */
	
}

//procustres
inline void ICP::minimize(const Pair& init_f){
	
	Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0);
	Eigen::Matrix3d M;
	
	unsigned int N = data_indices.size() + init_f.size();
	
	Eigen::MatrixXd model(3,N);
	Eigen::MatrixXd  data(3,N);
	
		
	//calcula os centroides
	int k=0;
	for(unsigned int i=0; i<N; i++){
		
		if (i<data_indices.size()){
			model(0,i) = cloud_m->points[ model_indices[i] ].x;
			model(1,i) = cloud_m->points[ model_indices[i] ].y;
			model(2,i) = cloud_m->points[ model_indices[i] ].z;
		
			data(0,i) = cloud_d->points[ data_indices[i] ].x*T(0,0) + cloud_d->points[ data_indices[i] ].y*T(0,1) + cloud_d->points[ data_indices[i] ].z*T(0,2) + T(0,3);
			data(1,i) = cloud_d->points[ data_indices[i] ].x*T(1,0) + cloud_d->points[ data_indices[i] ].y*T(1,1) + cloud_d->points[ data_indices[i] ].z*T(1,2) + T(1,3);
			data(2,i) = cloud_d->points[ data_indices[i] ].x*T(2,0) + cloud_d->points[ data_indices[i] ].y*T(2,1) + cloud_d->points[ data_indices[i] ].z*T(2,2) + T(2,3);
		}else{

			model(0,i) = cloud_m->points[ init_f[k].first ].x;
			model(1,i) = cloud_m->points[ init_f[k].first ].y;
			model(2,i) = cloud_m->points[ init_f[k].first ].z;
		
			data(0,i) = cloud_d->points[ init_f[k].second ].x*T(0,0) + cloud_d->points[ init_f[k].second ].y*T(0,1) + cloud_d->points[ init_f[k].second ].z*T(0,2) + T(0,3);
			data(1,i) = cloud_d->points[ init_f[k].second ].x*T(1,0) + cloud_d->points[ init_f[k].second ].y*T(1,1) + cloud_d->points[ init_f[k].second ].z*T(1,2) + T(1,3);
			data(2,i) = cloud_d->points[ init_f[k].second ].x*T(2,0) + cloud_d->points[ init_f[k].second ].y*T(2,1) + cloud_d->points[ init_f[k].second ].z*T(2,2) + T(2,3);	
			k++;
		}
		
		centroide_model += model.block(0,i,3,1);
		centroide_data  +=  data.block(0,i,3,1);
	}
	
	centroide_data = centroide_data/N;
	centroide_model = centroide_model/N;
	
	
  //subtrai os centroides aos dados
	for (unsigned int i=0; i<N; i++){
		model.block(0,i,3,1) -= centroide_model;
		data.block(0,i,3,1) -= centroide_data;
	}
	
	
	//Determina a transformacao
	M = data*model.transpose();
	
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
	
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	
	if (U.determinant()*V.determinant()<0) {
		for (int i=0; i<3; ++i) 
			V(i,2) *=-1;
	}
	
		
	Eigen::Matrix3d r = V * U.transpose();
	Eigen::Vector3d t = centroide_model - r * centroide_data;
	
	//~ T.block<3,3>(0,0) =  r*T.block<3,3>(0,0);
	//~ T.block<3,1>(0,3) += t; 
	T.block<3,1>(0,3) = T.block<3,3>(0,0)*t + T.block<3,1>(0,3); 
	T.block<3,3>(0,0) = T.block<3,3>(0,0)*r;
	
	
}