C++ Matrix4d::setIdentity方法代码示例

void doArcball(
	double * _viewMatrix, 
	double const * _rotationCenter, 
	double const * _projectionMatrix, 
	double const * _initialViewMatrix, 
	//double const * _currentViewMatrix,
	double const * _initialMouse, 
	double const * _currentMouse, 
	bool screenSpaceRadius,
	double radius)
{
	Eigen::Map<Eigen::Vector3d const> rotationCenter(_rotationCenter);
	Eigen::Map<Eigen::Matrix4d const> initialViewMatrix(_initialViewMatrix);
	//Eigen::Map<Eigen::Matrix4d const> currentViewMatrix(_currentViewMatrix);
	Eigen::Map<Eigen::Matrix4d const> projectionMatrix(_projectionMatrix);
	Eigen::Map<Eigen::Matrix4d> viewMatrix(_viewMatrix);

	Eigen::Vector2d initialMouse(_initialMouse);
	Eigen::Vector2d currentMouse(_currentMouse);
	
	Eigen::Quaterniond q;
	Eigen::Vector3d Pa, Pc;
	if (screenSpaceRadius) {
		double aspectRatio = projectionMatrix(1, 1) / projectionMatrix(0, 0);

		initialMouse(0) *= aspectRatio;
		currentMouse(0) *= aspectRatio;

		Pa = mapToSphere(initialMouse, radius);
		Pc = mapToSphere(currentMouse, radius);

		q.setFromTwoVectors(Pa - rotationCenter, Pc - rotationCenter);
	}
	else {
		Pa = mapToSphere(projectionMatrix.inverse(), initialMouse, rotationCenter, radius);
		Pc = mapToSphere(projectionMatrix.inverse(), currentMouse, rotationCenter, radius);

		Eigen::Vector3d a = Pa - rotationCenter, b = Pc - rotationCenter;

#if 0
		std::cout << "Mouse Initial Radius = " << sqrt(a.dot(a)) << " Current Radius = " << sqrt(b.dot(b)) << std::endl;
#endif

		q.setFromTwoVectors(a, b);
	}	

	Eigen::Matrix4d qMat4;
	qMat4.setIdentity();
	qMat4.topLeftCorner<3, 3>() = q.toRotationMatrix();

	Eigen::Matrix4d translate;
	translate.setIdentity();
	translate.topRightCorner<3, 1>() = -rotationCenter;

	Eigen::Matrix4d invTranslate;
	invTranslate.setIdentity();
	invTranslate.topRightCorner<3, 1>() = rotationCenter;
	
    viewMatrix = invTranslate * qMat4 * translate * initialViewMatrix;
}

//debug
void Dataset_analyzer::testetransformation(std::string file_pose_s, std::string file_pose_d, std::string ply_filename_s,std::string ply_filename_d){
	PCloud cloud_s;	
	PCloud cloud_d;	
	
	LoadCloudFromTXT(file_pose_s.data(), cloud_s);
	LoadCloudFromTXT(file_pose_d.data(), cloud_d);
	
	Eigen::Matrix4d Ts; Ts.setIdentity();
	Ts.block<3,3>(0,0) = (qs*qp).toRotationMatrix ();
	Ts.block<3,1>(0,3) = ts;
	
	Eigen::Matrix4d T = Computetransformation();
	//T.setIdentity();
	//T.block<3,3>(0,0) = (qs*qp).toRotationMatrix()*(qd*qp).toRotationMatrix().transpose();
	//T.block<3,1>(0,3) = td-ts;
	
	Eigen::Matrix4d Taux; Taux.setIdentity(); 
	Taux.block<3,3>(0,0) = T.block<3,3>(0,0)*Ts.block<3,3>(0,0);
	Taux.block<3,1>(0,3) = Ts.block<3,1>(0,3) + T.block<3,1>(0,3);
	
	PrintCloudToPLY(ply_filename_s.data(), cloud_s, Ts);
	PrintCloudToPLY(ply_filename_d.data(), cloud_d, Taux);
	
	//std::cout << T<< "\n";
	//std::cout << atan2(T(2,1), T(2,2)) << " " << asin(-T(2,0)) << " " << atan2(T(1,0), T(0,0)) << "\n";	
}		

Eigen::Matrix4d View::calcProjectionMatrix(Point2f frameBufferSize,float margin)
{
	float near=0,far=0;
	Point2d frustLL,frustUR;
	frustLL.x() = -imagePlaneSize * (1.0 + margin);
	frustUR.x() = imagePlaneSize * (1.0 + margin);
	double ratio =  ((double)frameBufferSize.y() / (double)frameBufferSize.x());
	frustLL.y() = -imagePlaneSize * ratio * (1.0 + margin);
	frustUR.y() = imagePlaneSize * ratio * (1.0 + margin);
	near = nearPlane;
	far = farPlane;
    
    
    // Borrowed from the "OpenGL ES 2.0 Programming" book
    Eigen::Matrix4d projMat;
    Point3d delta(frustUR.x()-frustLL.x(),frustUR.y()-frustLL.y(),far-near);
    projMat.setIdentity();
    projMat(0,0) = 2.0f * near / delta.x();
    projMat(1,0) = projMat(2,0) = projMat(3,0) = 0.0f;
    
    projMat(1,1) = 2.0f * near / delta.y();
    projMat(0,1) = projMat(2,1) = projMat(3,1) = 0.0f;
    
    projMat(0,2) = (frustUR.x()+frustLL.x()) / delta.x();
    projMat(1,2) = (frustUR.y()+frustLL.y()) / delta.y();
    projMat(2,2) = -(near + far ) / delta.z();
    projMat(3,2) = -1.0f;
    
    projMat(2,3) = -2.0f * near * far / delta.z();
    projMat(0,3) = projMat(1,3) = projMat(3,3) = 0.0f;
    
    return projMat;
}

Eigen::Matrix4d Dataset_analyzer::Computetransformation(){
	Eigen::Quaterniond qs_n = qs*qp; //change axis
	Eigen::Quaterniond qd_n = qd*qp; //change axis
	
	/*
	Eigen::Matrix4d Ts;	Ts.setIdentity();
	Eigen::Matrix4d Td; Td.setIdentity();
	
	Ts.block<3,3>(0,0) = qs_n.toRotationMatrix ();
	Ts.block<3,1>(0,3) = ts;
	
	Td.block<3,3>(0,0) = qd_n.toRotationMatrix ();
	Td.block<3,1>(0,3) = td;
		
	Eigen::Matrix4d T = Ts*Td.inverse();	
	*/

	Eigen::Quaterniond q12 = qd_n*qs_n.inverse(); //compute rotation s --> d
	
	Eigen::Matrix4d T; T.setIdentity();
	T.block<3,3>(0,0) = q12.toRotationMatrix ();
	T.block<3,1>(0,3) = td-ts;
	
	
	return T;
}

	demo(std::string _pointcloudtopic,std::string _cameras_filename){
		T.setIdentity();
		pub_pointcloud = n.advertise< sensor_msgs::PointCloud2 > ("/icp/world", 1);
		sub_pointcloud = n.subscribe(_pointcloudtopic.data(), 1, &demo::get_cloud_Callback, this);
		
		cameras_filename=_cameras_filename;
		f_th=4;
		
		cloud_key = PCloud(new Cloud());
	}

TEST(TransformationTestSuite, testConstructor)
{
  using namespace sm::kinematics;

  Transformation T_a_b;
  Eigen::Matrix4d T;
  T.setIdentity();
  
  sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity");

}

  Eigen::Matrix4d VertexPointXYZCov::covTransform(){
    EigenSolver<Matrix3d> solv(_cov);
    Matrix3d eigenVectors = solv.eigenvectors().real();

    Eigen::Matrix4d covGlTransform;
    covGlTransform.setIdentity();
    for(int i=0; i < 3; ++i)
      for(int j=0; j < 3; ++j)
        covGlTransform(i,j) = eigenVectors(i,j);

    for(int i=0; i < 3; ++i)
        covGlTransform(i,3) = estimate()(i);

    return covGlTransform; 
  }

TEST(UncertainTransformationTestSuite, testConstructor)
{
  using namespace sm::kinematics;

  UncertainTransformation T_a_b;
  Eigen::Matrix4d T;
  T.setIdentity();
  
  UncertainTransformation::covariance_t U;
  U.setZero();
  
  sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity");
  sm::eigen::assertNear(U, T_a_b.U(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating zero uncertainty");


}