C++ Transform::getBasis方法代码示例

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 MarkerVis::printTransform(tf::Transform  T)
{
    printf("[ %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f]\n", T.getBasis()[0][0], T.getBasis()[0][1], T.getBasis()[0][2], T.getBasis()[0][3],
            T.getBasis()[1][0], T.getBasis()[1][1], T.getBasis()[1][2], T.getBasis()[1][3],
            T.getBasis()[2][0], T.getBasis()[2][1], T.getBasis()[2][2], T.getBasis()[2][3],
            T.getBasis()[3][0], T.getBasis()[3][1], T.getBasis()[3][2], T.getBasis()[3][3]);
}

bool KlamptToROS(const RigidTransform& kT,tf::Transform& T)
{
  T.getOrigin().setValue(kT.t.x,kT.t.y,kT.t.z);
  Vector3 row;
  kT.R.getRow1(row);
  T.getBasis()[0].setValue(row.x,row.y,row.z);
  kT.R.getRow2(row);
  T.getBasis()[1].setValue(row.x,row.y,row.z);
  kT.R.getRow3(row);
  T.getBasis()[2].setValue(row.x,row.y,row.z);
  return true;
}

bool tfGreaterThan(const tf::Transform& tf, double dist, double angle)
{
  double d = tf.getOrigin().length();
  
  if (d >= dist) return true;

  double trace = tf.getBasis()[0][0] + tf.getBasis()[1][1] + tf.getBasis()[2][2];
  double a = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0)));

  if (a > angle) return true;
  
  return false;
}

MultiAgent3dNavigation::MultiAgent3dNavigation(const tf::Transform& world_to_cam, const tf::Transform& drone_to_marker, const tf::Transform& drone_to_front_cam, const ranav::TParam &p)
{
  this->params = p;
  motionModel = new ranav::Rotor3dMotionModel();
  motionModel->init(p);
  ekf = new ranav::EKF(motionModel);
  ekf->init(p);
  ttc = new ranav::TargetTrackingController();
  ttc->init(p);

  addOn3d.resize(motionModel->getNumAgents());

  this->world_to_cam = world_to_cam;
  this->drone_to_marker = drone_to_marker;
  this->drone_to_front_cam = drone_to_front_cam;

  double roll, pitch, yaw;

  world_to_cam.getBasis().getEulerYPR(yaw, pitch, roll);
  Eigen::Vector3d world_to_cam2d(world_to_cam.getOrigin().getX(),world_to_cam.getOrigin().getY(), yaw);
  world_to_cam2d += Eigen::Vector3d(2, 0, 0);  // HACK: move virtual 2D camera pose of tilted camera to have a circular field of view around (0, 0)
  tf::Quaternion q;
  q.setRPY(0, 0, world_to_cam2d(2));
  world_to_cam_2d = tf::Transform(q, tf::Vector3(world_to_cam2d(0), world_to_cam2d(1), 0));

  drone_to_marker.getBasis().getEulerYPR(yaw, pitch, roll);
  Eigen::Vector3d drone_to_marker2d(drone_to_marker.getOrigin().getX(), drone_to_marker.getOrigin().getY(), yaw);

  drone_to_front_cam.getBasis().getEulerYPR(yaw, pitch, roll);
  Eigen::Vector3d drone_to_front_cam2d(drone_to_front_cam.getOrigin().getX(), drone_to_front_cam.getOrigin().getY(), yaw);
  drone_to_front_cam2d += Eigen::Vector3d(0.8, 0, 0); // HACK: move virtual 2D camera pose of tilted camera to have a circular field of view around (0, 0)
  q.setRPY(0, 0, drone_to_front_cam2d(2));
  drone_to_front_cam_2d = tf::Transform(q, tf::Vector3(drone_to_front_cam2d(0), drone_to_front_cam2d(1), 0));

  ranav::AllModels models;
  ranav::Marker3dSensorModel *sm;
  sm = new ranav::Marker3dSensorModel(-1, 0);
  sm->init(p);
  sm->setCameraPose(world_to_cam2d); // HACK: overwrites parameter of config file
  sm->setMarkerPose(drone_to_marker2d); // HACK: overwrites parameter of config file
  models()[ranav::Index(-1, 0)] = sm;
  sm = new ranav::Marker3dSensorModel(0, 1);
  sm->init(p);
  sm->setCameraPose(drone_to_front_cam2d); // HACK: overwrites ...
  models()[ranav::Index(0, 1)] = sm;
  ttc->getTopology().setAllSensorModels(models);
  sensorModels = ttc->getTopology().getSensorModelsNonconst();
}

 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];
 }

static boost::numeric::ublas::matrix<double> transformAsMatrix(const tf::Transform& bt) 
{
   boost::numeric::ublas::matrix<double> outMat(4,4);
 
   //  double * mat = outMat.Store();
 
   double mv[12];
   bt.getBasis().getOpenGLSubMatrix(mv);
 
   btVector3 origin = bt.getOrigin();
 
   outMat(0,0)= mv[0];
   outMat(0,1)  = mv[4];
   outMat(0,2)  = mv[8];
   outMat(1,0)  = mv[1];
   outMat(1,1)  = mv[5];
   outMat(1,2)  = mv[9];
   outMat(2,0)  = mv[2];
   outMat(2,1)  = mv[6];
   outMat(2,2) = mv[10];
 
   outMat(3,0)  = outMat(3,1) = outMat(3,2) = 0;
   outMat(0,3) = origin.x();
   outMat(1,3) = origin.y();
   outMat(2,3) = origin.z();
   outMat(3,3) = 1;

 
   return outMat;
};

void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
                                            const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, 
                                            sensor_msgs::PointCloud & cloudOut) const
{
  tf::Vector3 origin = net_transform.getOrigin();
  tf::Matrix3x3 basis  = net_transform.getBasis();

  unsigned int length = cloudIn.points.size();

  // Copy relevant data from cloudIn, if needed
  if (&cloudIn != &cloudOut)
  {
    cloudOut.header = cloudIn.header;
    cloudOut.points.resize(length);
    cloudOut.channels.resize(cloudIn.channels.size());
    for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
      cloudOut.channels[i] = cloudIn.channels[i];
  }

  // Transform points
  cloudOut.header.stamp = target_time;
  cloudOut.header.frame_id = target_frame;
  for (unsigned int i = 0; i < length ; i++) {
    transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
  }
}

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;
	}
}

/** @brief Helper function to convert Eigen transformation to tf */
Eigen::Matrix4f EigenfromTf(tf::Transform trans)
{
	Eigen::Matrix4f eignMat;
	eignMat(0,3) = trans.getOrigin().getX();
	eignMat(1,3) = trans.getOrigin().getY();
	eignMat(2,3) = trans.getOrigin().getZ();
	for (int i=0;i<3;i++)
	{
		eignMat(i,0) = trans.getBasis().getRow(i).getX();
		eignMat(i,1) = trans.getBasis().getRow(i).getY();
		eignMat(i,2) = trans.getBasis().getRow(i).getZ();
	}
	eignMat(3,3) = 1;
	//ROS_INFO("trans: %f, %f, %f %f | %f, %f, %f %f | %f, %f, %f %f", eignMat(0,0), eignMat(0,1), eignMat(0,2), eignMat(0,3), eignMat(1,0), eignMat(1,1), eignMat(1,2), eignMat(1,3), eignMat(2,0), eignMat(2,1), eignMat(2,2), eignMat(2,3));
    return eignMat;
}

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 PathPlanningWidget::pointPosUpdated_slot(const tf::Transform& point_pos, const char* marker_name)
    {
        /*! When the user updates the position of the Way-Point or the User Interactive Marker, the information in the TreeView also needs to be updated to correspond to the current pose of the InteractiveMarkers.

        */
        QAbstractItemModel *model = ui_.treeView->model();

        ROS_INFO_STREAM("Updating marker name:"<<marker_name);

        tf::Vector3 p = point_pos.getOrigin();
        tfScalar rx,ry,rz;
        point_pos.getBasis().getRPY(rx,ry,rz,1);

        rx = RAD2DEG(rx);
        ry = RAD2DEG(ry);
        rz = RAD2DEG(rz);

      if((strcmp(marker_name,"add_point_button") == 0) || (atoi(marker_name)==0))
      {
          QString pos_s;
          pos_s = QString::number(p.x()) + "; " + QString::number(p.y()) + "; " + QString::number(p.z()) + ";";
          QString orient_s;
          orient_s = QString::number(rx) + "; " + QString::number(ry) + "; " + QString::number(rz) + ";";

          model->setData(model->index(0,0),QVariant("add_point_button"),Qt::EditRole);
          model->setData(model->index(0,1),QVariant(pos_s),Qt::EditRole);
          model->setData(model->index(0,2),QVariant(orient_s),Qt::EditRole);
      }
      else
      {

          int changed_marker = atoi(marker_name);
    //**********************update the positions and orientations of the children as well***********************************************************************************************
          QModelIndex ind = model->index(changed_marker, 0);
          QModelIndex chldind_pos = model->index(0, 0, ind);
          QModelIndex chldind_orient = model->index(1, 0, ind);

          //set the strings of each axis of the position
          QString pos_x = QString::number(p.x());
          QString pos_y = QString::number(p.y());
          QString pos_z = QString::number(p.z());

          //repeat that with the orientation
          QString orient_x = QString::number(rx);
          QString orient_y = QString::number(ry);
          QString orient_z = QString::number(rz);

          //second we add the current position information, for each position axis separately
          model->setData(model->index(0, 1, chldind_pos), QVariant(pos_x), Qt::EditRole);
          model->setData(model->index(1, 1, chldind_pos), QVariant(pos_y), Qt::EditRole);
          model->setData(model->index(2, 1, chldind_pos), QVariant(pos_z), Qt::EditRole);

          //second we add the current position information, for each position axis separately
          model->setData(model->index(0, 2, chldind_orient), QVariant(orient_x), Qt::EditRole);
          model->setData(model->index(1, 2, chldind_orient), QVariant(orient_y), Qt::EditRole);
          model->setData(model->index(2, 2, chldind_orient), QVariant(orient_z), Qt::EditRole);
//*****************************************************************************************************************************************************************************************
      }
    }

/**\fn void print_tf(tf::Transform xf)
 * \brief print a tf::Transform object
 * \param xf - a tf::Transform object to print
 * \return void
 *  \ingroup Kinematics
 */
void print_tf(tf::Transform xf) {
  tf::Matrix3x3 rr = xf.getBasis();
  tf::Vector3 vv = xf.getOrigin();
  std::stringstream ss;

  cout << fixed;
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) ss << rr[i][j] << "\t ";
    ss << vv[i] << endl;
  }
  ss << "0\t 0\t 0\t 1\n";
  log_msg("\n%s", ss.str().c_str());
}

Eigen::Affine3d transformTFToEigen(const tf::Transform &t) {
    Eigen::Affine3d e;
    for (int i = 0; i < 3; i++) {
        e.matrix()(i, 3) = t.getOrigin()[i];
        for (int j = 0; j < 3; j++) {
            e.matrix()(i, j) = t.getBasis()[i][j];
        }
    }
    // Fill in identity in last row
    for (int col = 0; col < 3; col++)
        e.matrix()(3, col) = 0;
    e.matrix()(3, 3) = 1;
    return e;
}

Eigen::Affine3d transformTFToEigen(const tf::Transform &t) {
    Eigen::Affine3d e;
    // treat the Eigen::Affine as a 4x4 matrix:
    for (int i = 0; i < 3; i++) {
        e.matrix()(i, 3) = t.getOrigin()[i]; //copy the origin from tf to Eigen
        for (int j = 0; j < 3; j++) {
            e.matrix()(i, j) = t.getBasis()[i][j]; //and copy 3x3 rotation matrix
        }
    }
    // Fill in identity in last row
    for (int col = 0; col < 3; col++)
        e.matrix()(3, col) = 0;
    e.matrix()(3, 3) = 1;
    return e;
}