本文整理汇总了C++中tf::Quaternion::getY方法的典型用法代码示例。如果您正苦于以下问题：C++ Quaternion::getY方法的具体用法？C++ Quaternion::getY怎么用？C++ Quaternion::getY使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf::Quaternion的用法示例。
在下文中一共展示了Quaternion::getY方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: MakeInteractiveMarker
void InteractiveMarkerArrow::MakeInteractiveMarker(std::string intMarkerName, tf::Quaternion qx_control, tf::Quaternion qy_control, tf::Quaternion qz_control)
{
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = "world";
int_marker.scale = 0.1;
int_marker.name = intMarkerName;
geometry_msgs::Pose pose;
tfSrv->get("world", intMarkerName + "_control", pose);
int_marker.pose = pose;
InteractiveMarkerArrow::MakeControl(int_marker);
int_marker.controls[0].interaction_mode = 7;
visualization_msgs::InteractiveMarkerControl control;
control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
tf::Transform tr;
tfSrv->get("world", "viceGripRotation", tr);
qx_control = tr.getRotation() * qx_control;
qy_control = tr.getRotation() * qy_control;
qz_control = tr.getRotation() * qz_control;
control.orientation.w = qx_control.getW();
control.orientation.x = qx_control.getX();
control.orientation.y = qx_control.getY();
control.orientation.z = qx_control.getZ();
control.name = "rotate_x";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_x";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
control.orientation.w = qz_control.getW();
control.orientation.x = qz_control.getX();
control.orientation.y = qz_control.getY();
control.orientation.z = qz_control.getZ();
control.name = "rotate_z";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_z";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
control.orientation.w = qy_control.getW();
control.orientation.x = qy_control.getX();
control.orientation.y = qy_control.getY();
control.orientation.z = qy_control.getZ();
control.name = "rotate_y";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_y";
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
intMarkerSrv->insert(int_marker);
intMarkerSrv->setCallback(int_marker.name, _processFeedBackTemp(boost::bind(&InteractiveMarkerArrow::ProcessFeedback, this, _1)));
intMarkerSrv->applyChanges();
}示例2: while
//.........这里部分代码省略.........
}
temp_counter++;
}
//New marker ?
if(existing == false)
{
index = marker_counter_;
markers_[index].marker_id = current_marker_id;
existing = true;
ROS_DEBUG_STREAM("New marker with ID: " << current_marker_id << " found");
}
// Change visibility flag of new marker
for(size_t j = 0;j < marker_counter_; j++)
{
for(size_t k = 0;k < temp_markers.size(); k++)
{
if(markers_[j].marker_id == temp_markers[k].id)
markers_[j].visible = true;
}
}
//------------------------------------------------------
// For existing marker do
//------------------------------------------------------
if((index < marker_counter_) && (first_marker_detected_ == true))
{
markers_[index].current_camera_tf = arucoMarker2Tf(temp_markers[i]);
markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();
const tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
markers_[index].current_camera_pose.position.x = marker_origin.getX();
markers_[index].current_camera_pose.position.y = marker_origin.getY();
markers_[index].current_camera_pose.position.z = marker_origin.getZ();
const tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
}
//------------------------------------------------------
// For new marker do
//------------------------------------------------------
if((index == marker_counter_) && (first_marker_detected_ == true))
{
markers_[index].current_camera_tf=arucoMarker2Tf(temp_markers[i]);
tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
markers_[index].current_camera_pose.position.x = marker_origin.getX();
markers_[index].current_camera_pose.position.y = marker_origin.getY();
markers_[index].current_camera_pose.position.z = marker_origin.getZ();
tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
// Naming - TFs
std::stringstream camera_tf_id;
std::stringstream camera_tf_id_old;
std::stringstream marker_tf_id_old;
示例3: complete_message_callback
//.........这里部分代码省略.........
else if(first_solution_found)
{
R_fc = first_R;
T_fc = first_T;
fc_found = true;
}
//if a solution was found will publish
// need to convert to pose message so use
if (fc_found)
{
// converting the rotation from a cv matrix to quaternion, first need it as a matrix3x3
R_fc_tf[0][0] = R_fc.at<double>(0,0);
R_fc_tf[0][1] = R_fc.at<double>(0,1);
R_fc_tf[0][2] = R_fc.at<double>(0,2);
R_fc_tf[1][0] = R_fc.at<double>(1,0);
R_fc_tf[1][1] = R_fc.at<double>(1,1);
R_fc_tf[1][2] = R_fc.at<double>(1,2);
R_fc_tf[2][0] = R_fc.at<double>(2,0);
R_fc_tf[2][1] = R_fc.at<double>(2,1);
R_fc_tf[2][2] = R_fc.at<double>(2,2);
std::cout << "Final R:\n" << R_fc << std::endl;
// converting the translation to a vector 3
T_fc_tf.setX(T_fc.at<double>(0,0));
T_fc_tf.setY(T_fc.at<double>(0,1));
T_fc_tf.setZ(T_fc.at<double>(0,2));
std::cout << "Final T :\n" << T_fc << std::endl;
// getting the rotation as a quaternion
R_fc_tf.getRotation(Q_fc_tf);
std::cout << "current orientation:" << "\n\tx:\t" << Q_fc_tf.getX()
<< "\n\ty:\t" << Q_fc_tf.getY()
<< "\n\tz:\t" << Q_fc_tf.getZ()
<< "\n\tw:\t" << Q_fc_tf.getW()
<< std::endl;
std::cout << "norm of quaternion:\t" << Q_fc_tf.length() << std::endl;
// getting the negated version of the quaternion for the check
Q_fc_tf_negated = tf::Quaternion(-Q_fc_tf.getX(),-Q_fc_tf.getY(),-Q_fc_tf.getZ(),-Q_fc_tf.getW());
std::cout << "negated orientation:" << "\n\tx:\t" << Q_fc_tf_negated.getX()
<< "\n\ty:\t" << Q_fc_tf_negated.getY()
<< "\n\tz:\t" << Q_fc_tf_negated.getZ()
<< "\n\tw:\t" << Q_fc_tf_negated.getW()
<< std::endl;
std::cout << "norm of negated quaternion:\t" << Q_fc_tf_negated.length() << std::endl;
// showing the last orientation
std::cout << "last orientation:" << "\n\tx:\t" << Q_fc_tf_last.getX()
<< "\n\ty:\t" << Q_fc_tf_last.getY()
<< "\n\tz:\t" << Q_fc_tf_last.getZ()
<< "\n\tw:\t" << Q_fc_tf_last.getW()
<< std::endl;
std::cout << "norm of last quaternion:\t" << Q_fc_tf_last.length() << std::endl;
// checking if the quaternion has flipped
Q_norm_current_diff = std::sqrt(std::pow(Q_fc_tf.getX() - Q_fc_tf_last.getX(),2.0)
+ std::pow(Q_fc_tf.getY() - Q_fc_tf_last.getY(),2.0)
+ std::pow(Q_fc_tf.getZ() - Q_fc_tf_last.getZ(),2.0)
+ std::pow(Q_fc_tf.getW() - Q_fc_tf_last.getW(),2.0));