C++ tf::Transform类代码示例

 void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
 {
   for (unsigned int i = 0; i < 3; ++i)
     k.p[i] = t.getOrigin()[i];
   for (unsigned int i = 0; i < 9; ++i)
     k.M.data[i] = t.getBasis()[i/3][i%3];
 }

void inline create_transform(tf::Transform &tf, float tx, float ty, float tz, float roll, float pitch, float yaw){
   tf::Quaternion q;
   q.setRPY(roll, pitch, yaw);

   tf.setOrigin(tf::Vector3( tx, ty, tz));
   tf.setRotation(q);
}

static void calculateDirectedTransform(const Eigen::Vector3d& start,
                                       const Eigen::Vector3d& stop,
                                       const tf::Vector3 z_axis,
                                       tf::Transform& transform)
{
  tf::Vector3 origin;
  tf::Quaternion pose;

  origin.setX(start(1));
  origin.setY(start(0));
  origin.setZ(start(2) - 0.05);

  transform.setOrigin(origin);

  tf::Vector3 n = toTF(stop) - toTF(start);
  n.normalize();
  tf::Vector3 z = z_axis;
  tf::Vector3 y;
  y = n.cross(z);
  y.normalize();
  z = n.cross(y);

  tf::Matrix3x3 rotation(
       z.getX(), n.getX(), y.getX(),
       z.getY(), n.getY(), y.getY(),
       z.getZ(), n.getZ(), y.getZ());

  transform.setBasis(rotation);
}

void TransformROSBridge::calculateTwist(const tf::Transform& _current_trans, const tf::Transform& _prev_trans, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt)
{
    // current rotation matrix
    tf::Matrix3x3 current_basis = _current_trans.getBasis();

    // linear twist
    tf::Vector3 current_origin = _current_trans.getOrigin();
    tf::Vector3 prev_origin = _prev_trans.getOrigin();
    _linear_twist = current_basis.transpose() * ((current_origin - prev_origin) / _dt);

    // angular twist
    // R = exp(omega_w*dt) * prev_R
    // omega_w is described in global coordinates in relationships of twist transformation.
    // it is easier to calculate omega using hrp functions than tf functions
    tf::Matrix3x3 prev_basis = _prev_trans.getBasis();
    double current_rpy[3], prev_rpy[3];
    current_basis.getRPY(current_rpy[0], current_rpy[1], current_rpy[2]);
    prev_basis.getRPY(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
    hrp::Matrix33 current_hrpR = hrp::rotFromRpy(current_rpy[0], current_rpy[1], current_rpy[2]);
    hrp::Matrix33 prev_hrpR = hrp::rotFromRpy(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
    hrp::Vector3 hrp_omega = current_hrpR.transpose() * hrp::omegaFromRot(current_hrpR * prev_hrpR.transpose()) / _dt;
    _angular_twist.setX(hrp_omega[0]);
    _angular_twist.setY(hrp_omega[1]);
    _angular_twist.setZ(hrp_omega[2]);

    return;
}

void
load_localize_transform()
{
	std::ifstream filed_transform;
	std::stringstream file_name;

	std::string path = ros::package::getPath("prx_localizer");
	file_name << path << "/transform/localize_transform.bin";
    filed_transform.open(file_name.str().c_str(), std::ofstream::binary);
    if (filed_transform.is_open())
    {
		tfScalar x, y, z, w;
		filed_transform.read((char *) &x, sizeof(tfScalar));
		filed_transform.read((char *) &y, sizeof(tfScalar));
		filed_transform.read((char *) &z, sizeof(tfScalar));
		filed_transform.read((char *) &w, sizeof(tfScalar));
		g_localize_transform.setRotation(tf::Quaternion(x, y, z, w));

		filed_transform.read((char *) &x, sizeof(tfScalar));
		filed_transform.read((char *) &y, sizeof(tfScalar));
		filed_transform.read((char *) &z, sizeof(tfScalar));
		g_localize_transform.setOrigin(tf::Vector3(x, y, z));

		filed_transform.close();
    }
    else
    	g_localize_transform = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(0.0, 0.0, 0.0));

    g_localize_transform_timestamp = ros::Time(0);
}

void add_edge(GraphOptimizer3D* optimizer, int id1, int id2, tf::Transform pose,
		double sigma_position, double sigma_orientation) {
	Vector6 p;
	double roll, pitch, yaw;
	MAT_to_RPY(pose.getBasis(), roll, pitch, yaw);
	p[0] = pose.getOrigin().x();
	p[1] = pose.getOrigin().y();
	p[2] = pose.getOrigin().z();
	p[3] = roll;
	p[4] = pitch;
	p[5] = yaw;

	Matrix6 m = Matrix6::eye(1.0);
	double ip = 1 / SQR(sigma_position);
	double io = 1 / SQR(sigma_orientation);
	m[0][0] = ip;
	m[1][1] = ip;
	m[2][2] = ip;
	m[3][3] = io;
	m[4][4] = io;
	m[5][5] = io;
	GraphOptimizer3D::Vertex* v1 = optimizer->vertex(id1);
	GraphOptimizer3D::Vertex* v2 = optimizer->vertex(id2);
	if (!v1) {
		cerr << "adding edge, id1=" << id1 << " not found" << endl;
	}
	if (!v2) {
		cerr << "adding edge, id2=" << id2 << " not found" << endl;
	}
	Transformation3 t = Transformation3::fromVector(p);
	GraphOptimizer3D::Edge* e = optimizer->addEdge(v1, v2, t, m);
	if (!e) {
		cerr << "adding edge failed" << endl;
	}
}

 void tfToPose(tf::Transform &trans, geometry_msgs::PoseStamped &msg)
 {
   tf::quaternionTFToMsg(trans.getRotation(), msg.pose.orientation);
   msg.pose.position.x = trans.getOrigin().x();
   msg.pose.position.y = trans.getOrigin().y();
   msg.pose.position.z = trans.getOrigin().z();
 }

void publish_faro_transform(tf::Transform faro_base)
{
  static tf::TransformBroadcaster br;

  //tf::Transform faro_base;

  //base_at_table - Actual,Circle,-291.052543573765,-100.393272630778,213.8235
  tf::Vector3 faro_origin(213.8235, 291.052543573765, -100.393272630778);
  faro_origin = faro_origin / 1000;
  faro_origin.setZ(faro_origin.z() + 0.10);
  faro_base.setOrigin(faro_origin);

  //+X_Coordinate System 1.i;0.9495;+Y_Coordinate System 1.i;0.3139;+Z_Coordinate System 1.i;-0.0008;
  //+X_Coordinate System 1.j;0.0007;+Y_Coordinate System 1.j;0.0005;+Z_Coordinate System 1.j;1.0000;
  //+X_Coordinate System 1.k;0.3139;+Y_Coordinate System 1.k-0.9495;+Z_Coordinate System 1.k;0.0002;
  tf::Matrix3x3 faro_rot;
  //faro_rot.setValue(0.9495, 0.3139, -0.0008, 0.0007, 0.0005, 1.0000, 0.3139, -0.9495, 0.0002);
  faro_rot.setValue(0.9495, 0.0007, 0.3139, 0.3139, 0.0005, -0.9495, -0.0008, 1.0000, 0.0002);
  tf::Quaternion faro_quat;
  double roll, pitch, yaw;
  faro_rot.getEulerYPR(yaw, pitch, roll);
  faro_quat.setRPY(roll, pitch, yaw);

  faro_base.setRotation(faro_quat);

  br.sendTransform(tf::StampedTransform(faro_base, ros::Time::now(), world_frame_, "faro_base"));

}

void imuCallback(const boost::shared_ptr<const sensor_msgs::Imu>& msg)
{
    //imu_msg = *msg;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = msg->header.frame_id;
    pose.header.stamp = msg->header.stamp;
    pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);

    geometry_msgs::PoseStamped imu_pose;
            
    try 
    {
        tf_listener->transformPose(odom_frame_id, pose, imu_pose);
    }
    catch(tf::TransformException &ex) 
    {
        ROS_ERROR("Squirtle Localization - %s - Error: %s", __FUNCTION__, ex.what());
        return;
    }
    
    tf::Quaternion q_imu;

    tf::quaternionMsgToTF(msg->orientation, q_imu);
    t_world_imu.setOrigin(tf::Vector3(0.0, 0.0, 0.0));

    tf::Quaternion temp = q_imu * tf::createQuaternionFromYaw(M_PI/2.0 + true_north_compensation);
    t_world_imu.setRotation(temp.normalized());
    
    ROS_INFO("Squirtle Localization - %s - IMU yaw in UTM - yaw:%lf", __FUNCTION__, tf::getYaw(msg->orientation) + M_PI/2.0 + true_north_compensation);

    tf::quaternionMsgToTF(imu_pose.pose.orientation, q_imu);
    t_odom_imu.setOrigin(tf::Vector3(imu_pose.pose.position.x, imu_pose.pose.position.y, imu_pose.pose.position.z));
    t_odom_imu.setRotation(q_imu);
}

void Person::update(int cx, int cy, tf::Transform transform, Time time)
{
	double dt = time.sec - last_update_time_.sec;
	Eigen::Matrix<double, 3, 4> tf_matrix;
	
	//Copy rotation matrix
	tf::Matrix3x3 rotation = transform.getBasis();
	for(int i=0; i < 3; i++)
	{
		tf::Vector3 row = rotation.getRow(i);
		tf_matrix(i,0) = row.x();	
		tf_matrix(i,1) = row.y(); 	
		tf_matrix(i,2) = row.z(); 	
	}

	//Copy translation matrix
	tf::Vector3 translation = transform.getOrigin();

	tf_matrix(0,3) = translation.x();	
	tf_matrix(1,3) = translation.y();	
	tf_matrix(2,3) = translation.z();	

	kf_->predict(dt);	
	kf_->update(cx, cy, tf_matrix, time);	
	last_update_time_ = time;

	life_time_ = 5;
	return;
}

void gpsCallback(const boost::shared_ptr<const sensor_msgs::NavSatFix>& msg)
{
    //gps_msg = *msg;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = msg->header.frame_id;
    pose.header.stamp = msg->header.stamp;
    pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);

    geometry_msgs::PoseStamped gps_pose;
            
    try 
    {
        tf_listener->transformPose(odom_frame_id, pose, gps_pose);
    }
    catch(tf::TransformException &ex) 
    {
        ROS_ERROR("Squirtle Localization - %s - Error: %s", __FUNCTION__, ex.what());
        return;
    }

    double x, y;
    int zone_number; char zone_letter;
    GPStoUTM(msg->latitude, msg->longitude, y, x, zone_number, zone_letter);
    t_world_gps.setOrigin(tf::Vector3(x, y, 5.0));
    t_world_gps.setRotation(tf::createQuaternionFromYaw(0.0));
    
    ROS_INFO("Squirtle Localization - %s - GPS position in UTM - x:%lf y:%lf", __FUNCTION__, x, y);

    t_odom_gps.setOrigin(tf::Vector3(gps_pose.pose.position.x, gps_pose.pose.position.y, gps_pose.pose.position.z));
    tf::Quaternion q_gps;
    tf::quaternionMsgToTF(gps_pose.pose.orientation, q_gps);
    t_odom_gps.setRotation(q_gps);
}

void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){		
	sensor_msgs::PointCloud2 cloud_in,cloud_out;
	projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_);
	rot_tf.header.frame_id = "/ChestLidar";
//	lidar mx28 axis aligned mode
//	rot_tf.transform.rotation.x = enc_tf_.getRotation().x();
//	rot_tf.transform.rotation.y = enc_tf_.getRotation().y();
//	rot_tf.transform.rotation.z = enc_tf_.getRotation().z();
//	rot_tf.transform.rotation.w = enc_tf_.getRotation().w();

//	lidar mx28 axis perpendicular modeg
	tf::Quaternion q1;
	q1.setRPY(-M_PI/2,0,0);
	tf::Transform m1(q1);
	tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w());
	tf::Transform m2(q2);

	tf::Transform m4;
	m4 = m2*m1; // rotate lidar axis and revolute mx28 axis
	rot_tf.transform.rotation.x = m4.getRotation().x();
	rot_tf.transform.rotation.y = m4.getRotation().y();
	rot_tf.transform.rotation.z = m4.getRotation().z();
	rot_tf.transform.rotation.w = m4.getRotation().w();
	
	tf2::doTransform(cloud_in,cloud_out,rot_tf);
	point_cloud_pub_.publish(cloud_out);		
	moving_now.data = dxl_ismove_;
	dxl_ismove_pub_.publish(moving_now);
}

 void transformKDLToTF(const KDL::Frame &k, tf::Transform &t)
 {
   t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
   t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
                          k.M.data[3], k.M.data[4], k.M.data[5],
                          k.M.data[6], k.M.data[7], k.M.data[8]));
 }

void
fromPose(const geometry_msgs::Pose &source, tf::Transform &dest)
{
    tf::Quaternion q(source.orientation.x, source.orientation.y, source.orientation.z, source.orientation.w);
    dest.setOrigin(tf::Vector3(source.position.x, source.position.y, source.position.z));
    dest.setRotation(q);
}

 void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
 {
   t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
   t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1),e.matrix()(0,2),
                            e.matrix()(1,0), e.matrix()(1,1),e.matrix()(1,2),
                            e.matrix()(2,0), e.matrix()(2,1),e.matrix()(2,2)));
 }