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

void ConvexPolygon::projectOnPlane(
    const Eigen::Affine3f& pose, Eigen::Affine3f& output)
{
    Eigen::Vector3f p(pose.translation());
    Eigen::Vector3f output_p;
    projectOnPlane(p, output_p);
    // ROS_INFO("[ConvexPolygon::projectOnPlane] p: [%f, %f, %f]",
    //          p[0], p[1], p[2]);
    // ROS_INFO("[ConvexPolygon::projectOnPlane] output_p: [%f, %f, %f]",
    //          output_p[0], output_p[1], output_p[2]);
    Eigen::Quaternionf rot;
    rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
                          coordinates().rotation() * Eigen::Vector3f::UnitZ());
    Eigen::Quaternionf coords_rot(coordinates().rotation());
    Eigen::Quaternionf pose_rot(pose.rotation());
    // ROS_INFO("[ConvexPolygon::projectOnPlane] rot: [%f, %f, %f, %f]",
    //          rot.x(), rot.y(), rot.z(), rot.w());
    // ROS_INFO("[ConvexPolygon::projectOnPlane] coords_rot: [%f, %f, %f, %f]",
    //          coords_rot.x(), coords_rot.y(), coords_rot.z(), coords_rot.w());
    // ROS_INFO("[ConvexPolygon::projectOnPlane] pose_rot: [%f, %f, %f, %f]",
    //          pose_rot.x(), pose_rot.y(), pose_rot.z(), pose_rot.w());
    // ROS_INFO("[ConvexPolygon::projectOnPlane] normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
    // Eigen::Affine3f::Identity() *
    // output.translation() = Eigen::Translation3f(output_p);
    // output.rotation() = rot * pose.rotation();
    //output = Eigen::Translation3f(output_p) * rot * pose.rotation();
    output = Eigen::Affine3f(rot * pose.rotation());
    output.pretranslate(output_p);
    // Eigen::Vector3f projected_point = output * Eigen::Vector3f(0, 0, 0);
    // ROS_INFO("[ConvexPolygon::projectOnPlane] output: [%f, %f, %f]",
    //          projected_point[0], projected_point[1], projected_point[2]);
}

void UrdfModelMarker::addInvisibleMeshMarkerControl(visualization_msgs::InteractiveMarker &int_marker, boost::shared_ptr<const Link> link, const std_msgs::ColorRGBA &color){
  visualization_msgs::InteractiveMarkerControl control;
  //    ps.pose = UrdfPose2Pose(link->visual->origin);
  visualization_msgs::Marker marker;

  //if (use_color) marker.color = color;
  marker.type = visualization_msgs::Marker::CYLINDER;
  double scale=0.01;
  marker.scale.x = scale;
  marker.scale.y = scale * 1;
  marker.scale.z = scale * 40;
  marker.color = color;
  //marker.pose = stamped.pose;
  //visualization_msgs::InteractiveMarkerControl control;
  boost::shared_ptr<Joint> parent_joint = link->parent_joint;
  Eigen::Vector3f origin_x(0,0,1);
  Eigen::Vector3f dest_x(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z);
  Eigen::Quaternionf qua;

  qua.setFromTwoVectors(origin_x, dest_x);
  marker.pose.orientation.x = qua.x();
  marker.pose.orientation.y = qua.y();
  marker.pose.orientation.z = qua.z();
  marker.pose.orientation.w = qua.w();

  control.markers.push_back( marker );
  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
  control.always_visible = true;

  int_marker.controls.push_back(control);
  return;
}

 geometry_msgs::PoseStamped SnapIt::alignPose(
   Eigen::Affine3f& pose, jsk_recognition_utils::ConvexPolygon::Ptr convex)
 {
   Eigen::Affine3f aligned_pose(pose);
   Eigen::Vector3f original_point(pose.translation());
   Eigen::Vector3f projected_point;
   convex->project(original_point, projected_point);
   
   Eigen::Vector3f normal = convex->getNormal();
   Eigen::Vector3f old_normal;
   old_normal[0] = pose(0, 2);
   old_normal[1] = pose(1, 2);
   old_normal[2] = pose(2, 2);
   Eigen::Quaternionf rot;
   if (normal.dot(old_normal) < 0) {
     normal = - normal;
   }
   rot.setFromTwoVectors(old_normal, normal);
   aligned_pose = aligned_pose * rot;
   aligned_pose.translation() = projected_point;
   Eigen::Affine3d aligned_posed;
   convertEigenAffine3(aligned_pose, aligned_posed);
   geometry_msgs::PoseStamped ret;
   tf::poseEigenToMsg(aligned_posed, ret.pose);
   return ret;
 }

void makeJointMarker(std::string jointName)
{
	boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(jointName);

	// The marker must be created in the parent frame so you don't get feedback when you move it
	visualization_msgs::InteractiveMarker marker;

	marker.scale = .125;
	marker.name = jointName;
	marker.header.frame_id = targetJoint->parent_link_name;

	geometry_msgs::Pose controlPose = hubo_motion_ros::toPose( targetJoint->parent_to_joint_origin_transform);
	marker.pose = controlPose;

	visualization_msgs::InteractiveMarkerControl control;

	Eigen::Quaternionf jointAxis;
	Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
	jointAxis.setFromTwoVectors(Eigen::Vector3f::UnitX(), axisVector);

	control.orientation.w = jointAxis.w();
	control.orientation.x = jointAxis.x();
	control.orientation.y = jointAxis.y();
	control.orientation.z = jointAxis.z();

	control.always_visible = true;
	control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;

	marker.controls.push_back(control);

	gIntServer->insert(marker);
	gIntServer->setCallback(marker.name, &processFeedback);
}

bool CObjTreePlugin::srvGetPlane(srs_env_model::GetPlane::Request &req, srs_env_model::GetPlane::Response &res)
{
    const objtree::Object *object = m_octree.object(req.object_id);
    //Object hasn't been found
    if(!object) return true;
    if(object->type() != objtree::Object::PLANE) return true;

    const objtree::Plane *plane = (const objtree::Plane*)object;

    res.plane.id = req.object_id;
    res.plane.pose.position.x = plane->pos().x;
    res.plane.pose.position.y = plane->pos().y;
    res.plane.pose.position.z = plane->pos().z;

    //Quaternion from normal
    Eigen::Vector3f normal(plane->normal().x, plane->normal().y, plane->normal().z);
    Eigen::Quaternionf q;
    q.setFromTwoVectors(upVector, normal.normalized());

    res.plane.pose.orientation.x = q.x();
    res.plane.pose.orientation.y = q.y();
    res.plane.pose.orientation.z = q.z();
    res.plane.pose.orientation.w = q.w();

    res.plane.scale.x = plane->boundingMax().x-plane->boundingMin().x;
    res.plane.scale.y = plane->boundingMax().y-plane->boundingMin().y;
    res.plane.scale.z = plane->boundingMax().z-plane->boundingMin().z;

    return true;
}

 void Plane::project(const Eigen::Affine3f& pose, Eigen::Affine3f& output)
 {
   Eigen::Vector3f p(pose.translation());
   Eigen::Vector3f output_p;
   project(p, output_p);
   Eigen::Quaternionf rot;
   rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
                         coordinates().rotation() * Eigen::Vector3f::UnitZ());
   output = Eigen::Affine3f::Identity() * Eigen::Translation3f(output_p) * rot;
 }

 void Plane::initializeCoordinates()
 {
   Eigen::Quaternionf rot;
   rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), normal_);
   double c = normal_[2];
   double z = 0.0;
   // ax + by + cz + d = 0
   // z = - d / c (when x = y = 0)
   if (c == 0.0) {             // its not good
     z = 0.0;
   }
   else {
     z = - d_ / c;
   }
   plane_coordinates_
     = Eigen::Affine3f::Identity() * Eigen::Translation3f(0, 0, z) * rot;
 }

void UrdfModelMarker::addMoveMarkerControl(visualization_msgs::InteractiveMarker &int_marker, boost::shared_ptr<const Link> link, bool root){
  visualization_msgs::InteractiveMarkerControl control;
  if(root){
    im_helpers::add6DofControl(int_marker,false);
    for(int i=0; i<int_marker.controls.size(); i++){
      int_marker.controls[i].always_visible = true;
    }
  }else{
    boost::shared_ptr<Joint> parent_joint = link->parent_joint;
    Eigen::Vector3f origin_x(1,0,0);
    Eigen::Vector3f dest_x(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z);
    Eigen::Quaternionf qua;

    qua.setFromTwoVectors(origin_x, dest_x);
    control.orientation.x = qua.x();
    control.orientation.y = qua.y();
    control.orientation.z = qua.z();
    control.orientation.w = qua.w();

    int_marker.scale = 0.5;

    switch(parent_joint->type){
    case Joint::REVOLUTE:
    case Joint::CONTINUOUS:
      control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
      int_marker.controls.push_back(control);
      break;
    case Joint::PRISMATIC:
      control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
      int_marker.controls.push_back(control);
      break;
    default:
      break;
    }
  }
}

  void TorusFinder::segment(
    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
  {
    if (!done_initialization_) {
      return;
    }
    boost::mutex::scoped_lock lock(mutex_);
    vital_checker_->poke();
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud
      (new pcl::PointCloud<pcl::PointNormal>);
    pcl::fromROSMsg(*cloud_msg, *cloud);
    jsk_recognition_utils::ScopedWallDurationReporter r
      = timer_.reporter(pub_latest_time_, pub_average_time_);
    if (voxel_grid_sampling_) {
      pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
      (new pcl::PointCloud<pcl::PointNormal>);
      pcl::VoxelGrid<pcl::PointNormal> sor;
      sor.setInputCloud (cloud);
      sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
      sor.filter (*downsampled_cloud);
      cloud = downsampled_cloud;
    }
    
    pcl::SACSegmentation<pcl::PointNormal> seg;
    if (use_normal_) {
      pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
      seg_normal.setInputNormals(cloud);
      seg = seg_normal;
    }

    
    seg.setOptimizeCoefficients (true);
    seg.setInputCloud(cloud);
    seg.setRadiusLimits(min_radius_, max_radius_);
    if (algorithm_ == "RANSAC") {
      seg.setMethodType(pcl::SAC_RANSAC);
    }
    else if (algorithm_ == "LMEDS") {
      seg.setMethodType(pcl::SAC_LMEDS);
    }
    else if (algorithm_ == "MSAC") {
      seg.setMethodType(pcl::SAC_MSAC);
    }
    else if (algorithm_ == "RRANSAC") {
      seg.setMethodType(pcl::SAC_RRANSAC);
    }
    else if (algorithm_ == "RMSAC") {
      seg.setMethodType(pcl::SAC_RMSAC);
    }
    else if (algorithm_ == "MLESAC") {
      seg.setMethodType(pcl::SAC_MLESAC);
    }
    else if (algorithm_ == "PROSAC") {
      seg.setMethodType(pcl::SAC_PROSAC);
    }
    
    seg.setDistanceThreshold (outlier_threshold_);
    seg.setMaxIterations (max_iterations_);
    seg.setModelType(pcl::SACMODEL_CIRCLE3D);
    if (use_hint_) {
      seg.setAxis(hint_axis_);
      seg.setEpsAngle(eps_hint_angle_);
    }
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    seg.segment(*inliers, *coefficients);
    NODELET_INFO("input points: %lu", cloud->points.size());
    if (inliers->indices.size() > min_size_) {
      // check direction. Torus should direct to origin of pointcloud
      // always.
      Eigen::Vector3f dir(coefficients->values[4],
                          coefficients->values[5],
                          coefficients->values[6]);
      if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
        dir = - dir;
      }
      
      Eigen::Affine3f pose = Eigen::Affine3f::Identity();
      Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
                                            coefficients->values[1],
                                            coefficients->values[2]);
      Eigen::Quaternionf rot;
      rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
      pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
      PCLIndicesMsg ros_inliers;
      ros_inliers.indices = inliers->indices;
      ros_inliers.header = cloud_msg->header;
      pub_inliers_.publish(ros_inliers);
      PCLModelCoefficientMsg ros_coefficients;
      ros_coefficients.values = coefficients->values;
      ros_coefficients.header = cloud_msg->header;
      pub_coefficients_.publish(ros_coefficients);
      jsk_recognition_msgs::Torus torus_msg;
      torus_msg.header = cloud_msg->header;
      tf::poseEigenToMsg(pose, torus_msg.pose);
      torus_msg.small_radius = 0.01;
      torus_msg.large_radius = coefficients->values[3];
      pub_torus_.publish(torus_msg);
      jsk_recognition_msgs::TorusArray torus_array_msg;
      torus_array_msg.header = cloud_msg->header;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    }
  }

  cv::Mat result;
  std::vector<tf::Vector3> results;
  interaction_msgs::ArmsPtr arms_msg(new interaction_msgs::Arms);
  arms_msg->arms.resize(g_hand_trackers.size());
  for (size_t i = 0; i < g_hand_trackers.size(); i++)
  {
    g_hand_trackers[i].first.predict(result);
    results.push_back(tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2)));
    arms_msg->arms[i].hand.rotation.w = 1;
  }

  int index;
  cv::Point3f measurement;
  tf::Quaternion q_hand;
  Eigen::Quaternionf q;
  tf::Quaternion q_rotate;
  q_rotate.setEuler(0, 0, M_PI);
  int last_index = -1;
  for(size_t i = 0; i < hand_positions.size(); i++)
  {
    index = closestPoint(hand_positions[i], results);
    if(last_index == index)
      continue;
    last_index = index;
    measurement.x = hand_positions[i].x();
    measurement.y = hand_positions[i].y();
    measurement.z = hand_positions[i].z();
    g_hand_trackers[index].first.update(measurement, result);
    results[index] = tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2));

    q.setFromTwoVectors(Eigen::Vector3f(1, 0, 0),
                        Eigen::Vector3f(arm_directions[i].x(), arm_directions[i].y(), arm_directions[i].z()));
    tf::Quaternion q_tf(q.x(), q.y(), q.z(), q.w());
    q_hand = q_tf * q_rotate;
    arms_msg->arms[index].hand.rotation.x = q_hand.x();
    arms_msg->arms[index].hand.rotation.y = q_hand.y();
    arms_msg->arms[index].hand.rotation.z = q_hand.z();
    arms_msg->arms[index].hand.rotation.w = q_hand.w();
  }

  for(size_t i = 0; i < results.size(); i++)
  {
    g_hand_trackers[i].first.updateState();
    arms_msg->arms[i].arm_id = g_hand_trackers[i].first.id();
    arms_msg->arms[i].hand.translation.x = results[i].x();
    arms_msg->arms[i].hand.translation.y = results[i].y();
    arms_msg->arms[i].hand.translation.z = results[i].z();

    //prepare markers if needed
    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      geometry_msgs::Point marker_point;
      marker_point.x = results[i].x();
      marker_point.y = results[i].y();
      marker_point.z = results[i].z();
      g_hand_trackers[i].second.push_back(marker_point);
      if(g_hand_trackers[i].second.size() > HAND_HISTORY_SIZE)
        g_hand_trackers[i].second.pop_front();
    }

#if PUBLISH_TRANSFORM
    tf::Transform hand_tf;
    hand_tf.setOrigin(results[i]);

void CObjTreePlugin::showObject(unsigned int id)
{
    const objtree::Object *object = m_octree.object(id);
    if(!object) return;

    char name[64];
    snprintf(name, sizeof(name), "imn%u", id);

    switch(object->type())
    {
        case objtree::Object::ALIGNED_BOUNDING_BOX:
        {
            objtree::BBox *box = (objtree::BBox*)object;

            srs_interaction_primitives::AddBoundingBox addBoxSrv;

            addBoxSrv.request.frame_id = IM_SERVER_FRAME_ID;
            addBoxSrv.request.name = name;
            addBoxSrv.request.description = name;

            addBoxSrv.request.pose.position.x = box->box().x+box->box().w/2;
            addBoxSrv.request.pose.position.y = box->box().y+box->box().h/2;
            addBoxSrv.request.pose.position.z = box->box().z+box->box().d/2;

            addBoxSrv.request.pose.orientation.x = 0.0f;
            addBoxSrv.request.pose.orientation.y = 0.0f;
            addBoxSrv.request.pose.orientation.z = 0.0f;
            addBoxSrv.request.pose.orientation.w = 1.0f;

            addBoxSrv.request.scale.x = box->box().w;
            addBoxSrv.request.scale.y = box->box().h;
            addBoxSrv.request.scale.z = box->box().d;

            addBoxSrv.request.color.r = 1.0;
            addBoxSrv.request.color.g = addBoxSrv.request.color.b = 0.0;

            addBoxSrv.request.color.a = 1.0;

            m_clientAddBoundingBox.call(addBoxSrv);
        }
        break;

        case objtree::Object::GENERAL_BOUNDING_BOX:
        {
            objtree::GBBox *box = (objtree::GBBox*)object;

            srs_interaction_primitives::AddBoundingBox addBoxSrv;

            addBoxSrv.request.frame_id = IM_SERVER_FRAME_ID;
            addBoxSrv.request.name = name;
            addBoxSrv.request.description = name;

            addBoxSrv.request.pose.position.x = box->position().x;
            addBoxSrv.request.pose.position.y = box->position().y;
            addBoxSrv.request.pose.position.z = box->position().z;

            addBoxSrv.request.pose.orientation.x = box->orientation().x;
            addBoxSrv.request.pose.orientation.y = box->orientation().y;
            addBoxSrv.request.pose.orientation.z = box->orientation().z;
            addBoxSrv.request.pose.orientation.w = box->orientation().w;

            addBoxSrv.request.scale.x = box->scale().x;
            addBoxSrv.request.scale.y = box->scale().y;
            addBoxSrv.request.scale.z = box->scale().z;

            addBoxSrv.request.color.r = 1.0;
            addBoxSrv.request.color.g = addBoxSrv.request.color.b = 0.0;

            addBoxSrv.request.color.a = 1.0;

            m_clientAddBoundingBox.call(addBoxSrv);
        }
        break;

        case objtree::Object::PLANE:
        {
            objtree::Plane *plane = (objtree::Plane*)object;

            srs_interaction_primitives::AddPlane addPlaneSrv;

            addPlaneSrv.request.frame_id = IM_SERVER_FRAME_ID;
            addPlaneSrv.request.name = name;
            addPlaneSrv.request.description = name;

            addPlaneSrv.request.pose.position.x = plane->pos().x;
            addPlaneSrv.request.pose.position.y = plane->pos().y;
            addPlaneSrv.request.pose.position.z = plane->pos().z;

            //Quaternion from normal
            Eigen::Vector3f normal(plane->normal().x, plane->normal().y, plane->normal().z);
            Eigen::Quaternionf q;
            q.setFromTwoVectors(upVector, normal.normalized());

            addPlaneSrv.request.pose.orientation.x = q.x();
            addPlaneSrv.request.pose.orientation.y = q.y();
            addPlaneSrv.request.pose.orientation.z = q.z();
            addPlaneSrv.request.pose.orientation.w = q.w();

            addPlaneSrv.request.scale.x = plane->boundingMax().x-plane->boundingMin().x;
            addPlaneSrv.request.scale.y = plane->boundingMax().y-plane->boundingMin().y;
//.........这里部分代码省略.........