C++ Quaternionf::toRotationMatrix方法代码示例

  void DrawableCloud::draw() {
    if(_parameter->show() && _cloud) {
      glPushMatrix();
      glMultMatrixf(_transformation.data());
      if(_drawablePoints)
	_drawablePoints->draw();
      if(_drawableNormals)
	_drawableNormals->draw();
      if(_drawableCovariances)
	_drawableCovariances->draw();
      if(_drawableCorrespondences)
	_drawableCorrespondences->draw();

      glPushMatrix();
      Eigen::Isometry3f sensorOffset;
      sensorOffset.translation() = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
      Eigen::Quaternionf quaternion = Eigen::Quaternionf(-.5f, -0.5f, 0.5f, 0.5f);
      sensorOffset.linear() = quaternion.toRotationMatrix();
      sensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
      glMultMatrixf(sensorOffset.data());
      glColor4f(1,0,0,0.5);
      glPopMatrix();

      glPopMatrix();
    }
  }

void generateTF(int trans, Eigen::Quaternionf &R,  Eigen::Vector3f &t, Eigen::Matrix3f &mR)
{
  float strengthR=(trans%3)*0.05;
  trans/=3;
  float strengthT=(trans%3)*0.05;
  trans/=3;

  int temp=trans%8;
  Eigen::Vector3f axis;
  axis(0) = temp&1?1:0;
  axis(1) = temp&2?1:0;
  axis(2) = temp&4?1:0;
  trans/=8;

  Eigen::AngleAxisf aa(strengthR,axis);
  R = aa.toRotationMatrix();
  mR= R.toRotationMatrix();
  R = mR;

  t(0) = temp&1?1:0;
  if(temp&2) t(0)=-t(0);
  t(1) = temp&4?1:0;
  if(temp&8) t(1)=-t(1);
  t(2) = temp&16?1:0;
  if(temp&32) t(2)=-t(2);
  trans/=64;

  t*=strengthT;
}

void transform(pcl::PointCloud<pcl::PointXYZ> &pc, Eigen::Quaternionf &R,  Eigen::Vector3f &t)
{
  Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++)
      m(i,j) = R.toRotationMatrix()(i,j);
  m.col(3).head<3>() = t;

  pcl::transformPointCloud(pc,pc,m);
}

void CPclView::addCameraMesh(const pcl::PointXYZ& position, const Eigen::Quaternionf& rotation,
                             const QString& id)
{
  pcl::PolygonMesh cameraMesh;

  initzializeCameraMeshCloud(&cameraMesh.cloud);

  Eigen::Vector3f cameraShapeVertices[CAMERA_SHAPE_N_VERTICES];

  cameraShapeVertices[0] = Eigen::Vector3f( 0.f,  0.f, 0.f);
  cameraShapeVertices[1] = Eigen::Vector3f(-1.f, -1.f, 1.f);
  cameraShapeVertices[2] = Eigen::Vector3f(-1.f,  1.f, 1.f);
  cameraShapeVertices[3] = Eigen::Vector3f( 1.f, -1.f, 1.f);
  cameraShapeVertices[4] = Eigen::Vector3f( 1.f,  1.f, 1.f);

  Eigen::Vector3f cameraBasePos(position.x, position.y, position.z);
  Eigen::Quaternionf quat = rotation.normalized();
  Eigen::Matrix3f rotationMatrix = quat.toRotationMatrix();

  for(int i = 0; i < CAMERA_SHAPE_N_VERTICES; i++)
  {
    cameraShapeVertices[i] = rotationMatrix * cameraShapeVertices[i];
    cameraShapeVertices[i] += cameraBasePos;

    for(unsigned int j = 0; j < 3; j++)
    {
      reinterpret_cast<float*>(cameraMesh.cloud.data.data())[i * 3 + j] = cameraShapeVertices[i][j];
    }
  }

  pcl::Vertices v;

  v.vertices.push_back(1);
  v.vertices.push_back(2);
  v.vertices.push_back(4);
  v.vertices.push_back(3);
  cameraMesh.polygons.push_back(v);

  v.vertices.clear();
  v.vertices.push_back(0);
  v.vertices.push_back(1);
  v.vertices.push_back(3);
  v.vertices.push_back(0);
  cameraMesh.polygons.push_back(v);

  v.vertices.clear();
  v.vertices.push_back(0);
  v.vertices.push_back(2);
  v.vertices.push_back(4);
  v.vertices.push_back(0);
  cameraMesh.polygons.push_back(v);

  mpPclVisualizer->addPolylineFromPolygonMesh(cameraMesh, id.toStdString());
}

void addCoordinateFrame (geometry_msgs::Pose pose) {
	cloud_viewer.addCoordinateSystem (0.5);
	Eigen::Vector3f t;
	t(0) = pose.position.x;
	t(1) = pose.position.y;
	t(2) = -pose.position.z; // PCL seems to have a strange Z axis
	Eigen::Quaternionf q;
	q.x() = pose.orientation.x;
	q.y() = pose.orientation.y;
	q.z() = pose.orientation.z;
	q.w() = pose.orientation.w;
	Eigen::Matrix3f R = q.toRotationMatrix();
	Eigen::Vector3f r = R.eulerAngles(0, 1, 2);
	Eigen::Affine3f A;
	pcl::getTransformation (t(0), t(1), t(2), r(0), r(1), r(2), A);
	cloud_viewer.addCoordinateSystem (0.2, A);
}

  void ImageMessageListener::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) {
    cerr << "got camera matrix for topic: " << _camera_info_topic << endl;
    if (_listener) {
      char buffer[1024];
      buffer[0] = 0;

      tf::StampedTransform transform;
      try{
        _listener->waitForTransform(_base_link_frame_id, msg->header.frame_id, 
                                    msg->header.stamp, 
                                    ros::Duration(0.5) );
        _listener->lookupTransform (_base_link_frame_id, msg->header.frame_id, 
                                    msg->header.stamp, 
                                    transform);
      }
      catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        return;
      }

      Eigen::Quaternionf q;
      q.x() = transform.getRotation().x();
      q.y() = transform.getRotation().y();
      q.z() = transform.getRotation().z();
      q.w() = transform.getRotation().w();
      _camera_offset.linear()=q.toRotationMatrix();
      _camera_offset.translation()=Eigen::Vector3f(transform.getOrigin().x(),
                                                   transform.getOrigin().y(),
                                                   transform.getOrigin().z());
    }      
    int i;
    for (int r=0; r<3; r++)
      for (int c=0; c<3; c++, i++)
        _K(r,c) = msg->K[i];
    _K.row(2) << 0,0,1;
    _has_camera_matrix = true;
    _camera_info_subscriber.shutdown();
  }

/**
 * @brief Callback for feedback subscriber for getting the transformation of moved markers
 *
 * @param feedback subscribed from geometry_map/map/feedback
 */
void ShapeVisualization::setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)//,const cob_3d_mapping_msgs::Shape& shape)
{

  cob_3d_mapping_msgs::ShapeArray map_msg;
  map_msg.header.frame_id="/map";
  map_msg.header.stamp = ros::Time::now();

  int shape_id,index;
  index=-1;
  stringstream name(feedback->marker_name);

  Eigen::Quaternionf quat;

  Eigen::Matrix3f rotationMat;
  Eigen::MatrixXf rotationMatInit;

  Eigen::Vector3f normalVec;
  Eigen::Vector3f normalVecNew;
  Eigen::Vector3f newCentroid;
  Eigen::Matrix4f transSecondStep;


  if (feedback->marker_name != "Text"){
    name >> shape_id ;
    cob_3d_mapping::Polygon p;

    for(int i=0;i<sha.shapes.size();++i)
    {
    	if (sha.shapes[i].id == shape_id)
	{
		index = i;
	}

    }


    // temporary fix.
    //do nothing if index of shape is not found
    // this is not supposed to occur , but apparently it does
    if(index==-1){

    ROS_WARN("shape not in map array");
    return;
	}


    cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

    if (feedback->event_type == 2 && feedback->menu_entry_id == 5){
      quatInit.x() = (float)feedback->pose.orientation.x ;           //normalized
      quatInit.y() = (float)feedback->pose.orientation.y ;
      quatInit.z() = (float)feedback->pose.orientation.z ;
      quatInit.w() = (float)feedback->pose.orientation.w ;

      oldCentroid (0) = (float)feedback->pose.position.x ;
      oldCentroid (1) = (float)feedback->pose.position.y ;
      oldCentroid (2) = (float)feedback->pose.position.z ;

      quatInit.normalize() ;

      rotationMatInit = quatInit.toRotationMatrix() ;

      transInit.block(0,0,3,3) << rotationMatInit ;
      transInit.col(3).head(3) << oldCentroid(0) , oldCentroid(1), oldCentroid(2) ;
      transInit.row(3) << 0,0,0,1 ;

      transInitInv = transInit.inverse() ;
      Eigen::Affine3f affineInitFinal (transInitInv) ;
      affineInit = affineInitFinal ;

      std::cout << "transInit : " << "\n"    << affineInitFinal.matrix() << "\n" ;
    }

    if (feedback->event_type == 5){
      /* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
      //string strName(feedback->marker_name);
      //strName.erase(strName.begin(),strName.begin()+7);
//      stringstream name(strName);
	stringstream name(feedback->marker_name);

      name >> shape_id ;
      cob_3d_mapping::Polygon p;
      cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

      quat.x() = (float)feedback->pose.orientation.x ;           //normalized
      quat.y() = (float)feedback->pose.orientation.y ;
      quat.z() = (float)feedback->pose.orientation.z ;
      quat.w() = (float)feedback->pose.orientation.w ;

      quat.normalize() ;

      rotationMat = quat.toRotationMatrix() ;

      normalVec << sha.shapes.at(index).params[0],                   //normalized
          sha.shapes.at(index).params[1],
//.........这里部分代码省略.........

int LoadPCD::compute()
{
	//for each selected filename
	for (int k = 0; k < m_filenames.size(); ++k)
	{
		Eigen::Vector4f origin;
		Eigen::Quaternionf orientation;

		QString filename = m_filenames[k];

		boost::shared_ptr<PCLCloud> cloud_ptr_in = loadSensorMessage(filename, origin, orientation);

		if (!cloud_ptr_in) //loading failed?
			return 0;

		PCLCloud::Ptr cloud_ptr;
		if (!cloud_ptr_in->is_dense) //data may contain nans. Remove them
		{
			//now we need to remove nans
			pcl::PassThrough<PCLCloud> passFilter;
			passFilter.setInputCloud(cloud_ptr_in);

			cloud_ptr = PCLCloud::Ptr(new PCLCloud);
			passFilter.filter(*cloud_ptr);
		}
		else
		{
			cloud_ptr = cloud_ptr_in;
		}

		//now we construct a ccGBLSensor with these characteristics
		ccGBLSensor * sensor = new ccGBLSensor;

		// get orientation as rot matrix
		Eigen::Matrix3f eigrot = orientation.toRotationMatrix();

		// and copy it into a ccGLMatrix
		ccGLMatrix ccRot;
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				ccRot.getColumn(j)[i] = eigrot(i,j);
			}
		}

		ccRot.getColumn(3)[3] = 1.0;

		// now in a format good for CloudComapre
		//ccGLMatrix ccRot = ccGLMatrix::FromQuaternion(orientation.coeffs().data());

		//ccRot = ccRot.transposed();
		ccRot.setTranslation(origin.data());

		sensor->setRigidTransformation(ccRot);
		sensor->setDeltaPhi(static_cast<PointCoordinateType>(0.05));
		sensor->setDeltaTheta(static_cast<PointCoordinateType>(0.05));
		sensor->setVisible(true);

		//uncertainty to some default
		sensor->setUncertainty(static_cast<PointCoordinateType>(0.01));

		ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCloud();
		if (!out_cloud)
			return -31;

		sensor->setGraphicScale(out_cloud->getBB().getDiagNorm() / 10);

		//do the projection on sensor
		ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(out_cloud);
		int errorCode;
		CCLib::SimpleCloud* projectedCloud = sensor->project(cloud,errorCode,true);
		if (projectedCloud)
		{
			//DGM: we don't use it but we still have to delete it!
			delete projectedCloud;
			projectedCloud = 0;
		}

		QString cloud_name = QFileInfo(filename).baseName();
		out_cloud->setName(cloud_name);

		QFileInfo fi(filename);
		QString containerName = QString("%1 (%2)").arg(fi.fileName()).arg(fi.absolutePath());

		ccHObject* cloudContainer = new ccHObject(containerName);
		out_cloud->addChild(sensor);
		cloudContainer->addChild(out_cloud);

		emit newEntity(cloudContainer);
	}

	return 1;
}

  void ImageMessageListener::imageCallback(const sensor_msgs::ImageConstPtr& in_msg) {
    static int counter = 0;     // counter to add periodic line breaks
    _image_deque.push_back(*in_msg);
    if (_image_deque.size()<_deque_length){
      return;
    }
    sensor_msgs::Image msg = _image_deque.front();
    _image_deque.pop_front();
    
    
    if (_listener) {
      tf::StampedTransform transform;
      try{
        _listener->waitForTransform(_odom_frame_id, 
                                    _base_link_frame_id, 
                                    msg.header.stamp, 
                                    ros::Duration(0.5) );
        _listener->lookupTransform (_odom_frame_id, 
                                    _base_link_frame_id, 
                                    msg.header.stamp, 
                                    transform);
      }
      catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
      }
      Eigen::Quaternionf q;
      q.x() = transform.getRotation().x();
      q.y() = transform.getRotation().y();
      q.z() = transform.getRotation().z();
      q.w() = transform.getRotation().w();
      _odom_pose.linear()=q.toRotationMatrix();
      _odom_pose.translation()=Eigen::Vector3f(transform.getOrigin().x(),
                                               transform.getOrigin().y(),
                                               transform.getOrigin().z());
    }      


    cv_bridge::CvImageConstPtr bridge;
    bridge = cv_bridge::toCvCopy(msg,msg.encoding);

  
    if (! _has_camera_matrix){
      cerr << "received: " << _image_topic << " waiting for camera matrix" << endl;
      return;
    }
    _cv_image = bridge->image.clone();
 
  
    PinholeImageMessage* img_msg = new PinholeImageMessage(_image_topic, msg.header.frame_id,  msg.header.seq, msg.header.stamp.toSec());
    img_msg->setImage(_cv_image);
    img_msg->setOffset(_camera_offset);
    img_msg->setCameraMatrix(_K);
    img_msg->setDepthScale(_depth_scale);
  
    if (_imu_interpolator && _listener) {
      Eigen::Isometry3f imu = Eigen::Isometry3f::Identity();
      Eigen::Quaternionf q_imu;
      if (!_imu_interpolator->getOrientation(q_imu, msg.header.stamp.toSec()))
        return;
      imu.linear() = q_imu.toRotationMatrix();
      if (_verbose) {
        cerr << "i";
        counter++;
        if( counter % 1024 == 0 )
          cerr << endl;
      }
      img_msg->setImu(imu);
    } else if (_listener) {
      img_msg->setOdometry(_odom_pose);
      if (_verbose)
        cerr << "o";
        counter++;
        if( counter % 1024 == 0 )
          cerr << endl;
    } else {
      if (_verbose)
        cerr << "x";
        counter++;
        if( counter % 1024 == 0 )
          cerr << endl;
    }
    _sorter->insertMessage(img_msg);
  }

int main(int argc, char **argv)
{	
  	std::vector<int> p_file_indices_conf = pcl::console::parse_file_extension_argument (argc, argv, ".conf");
    std::string output_directory = ".";
    pcl::console::parse_argument (argc, argv, "--directory", output_directory); 

  	for (int i = 0; i < p_file_indices_conf.size(); ++i)
  	{
  		QFile conf_file(argv[p_file_indices_conf[i]]);
  		if(conf_file.open(QIODevice::ReadOnly)) 
  		{
  			QTextStream in(&conf_file);
  			while(!in.atEnd()) {
  			    QString line = in.readLine();
  			    //qDebug() << line;
  			    QStringList list = line.split(" ");
  			    if (list[0] == "camera" || list[0] == "mesh") continue;
  			    if (list[0] == "bmesh")
  			    {
  			    	QString ply_file = list[1];
  			    	float tx = list[2].toFloat();
  			    	float ty = list[3].toFloat();
  			    	float tz = list[4].toFloat();
  			    	float qi = list[5].toFloat();
  			    	float qj = list[6].toFloat();
  			    	float qk = list[7].toFloat();
  			    	float ql = list[8].toFloat();
  			    	Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
  			    	Eigen::Vector3f translation = Eigen::Vector3f(tx, ty, tz);
  			    	Eigen::Quaternionf quaternion = Eigen::Quaternionf(qi, qj, qk, ql);
  			    	transformation.block<3,3>(0,0) = quaternion.toRotationMatrix();
  			    	transformation.block<3,1>(0,3) = translation;

              QFileInfo fileInfo(ply_file);
  			    	CloudIO::exportTransformation(QString(output_directory.c_str()) + "/" + fileInfo.completeBaseName() + ".tf", transformation);
  			    }
  			}
  		}
  	}

    // pcl::PLYReader reader;
    // pcl::PolygonMesh mesh;
    // reader.read("dragonStandRight_0_out.ply", mesh);

    // std::cout << mesh.cloud.height << std::endl;
    // std::cout << mesh.cloud.width << std::endl;

    // QFileInfo info("dragonStandRight_0_out.ply");
    // pcl::io::savePLYFile((info.path() + "/" + info.completeBaseName() + "_transformed.ply").toStdString(), mesh);

  	// pcl::PLYReader reader;
  	// pcl::PointCloud<pcl::PointXYZRGBNormal> cloud;

  	// reader.read("bun000.ply", cloud);

  	// std::cout << cloud << std::endl;

  	// // pcl::PLYWriter writer;
  	// // writer.write("bun000_out_out.ply", cloud);

  	// pcl::PCDWriter writer;
  	// writer.write("bun000.pcd", cloud);


  	// pcl::PLYReader reader;
  	// pcl::PolygonMesh mesh;
  	// reader.read("bun000_meshlab.ply", mesh);

  	// std::cout << mesh.cloud.height << std::endl;
  	// std::cout << mesh.cloud.width << std::endl;

  	// pcl::visualization::PCLVisualizer visualizer;
  	// visualizer.addPolygonMesh(mesh);

  	// visualizer.spin();

  	// std::cout << mesh.polygons.size() << std::endl;
  	// for (int i = 0; i < 10; ++i)
  	// {
  	// 	std::cout << mesh.polygons[i].vertices.size() << std::endl;

  	// 	for (int j = 0; j < mesh.polygons[i].vertices.size(); ++j)
  	// 	{
  	// 		std::cout << mesh.polygons[i].vertices[j] << " ";
  	// 	}
  	// 	std::cout << std::endl;
  	// }


	// for (int i = 0; i < p_file_indices_conf.size(); ++i)
	// {
	// 	CloudDataPtr cloudData(new CloudData);
	// 	Eigen::Matrix4f transformation;
	// 	BoundariesPtr boundaries(new Boundaries);

	// 	QString fileName = QString(argv[p_file_indices_ply[i]]);
	// 	CloudIO::importCloudData(fileName, cloudData);
	// 	CloudIO::importTransformation(fileName, transformation);
	// 	CloudIO::importBoundaries(fileName, boundaries);
	// 	Cloud* cloud = cloudManager->addCloud(cloudData, Cloud::fromIO, fileName, transformation);
//.........这里部分代码省略.........