void Vertex::draw_with_name(unsigned int idx, const Matrix44& adjust_matrix)
{
glPushName(idx);
Vec4 tmp(position_(0), position_(1), position_(2),1.);
Vec4 point_to_show = adjust_matrix * tmp;
glRasterPos3f(point_to_show(0), point_to_show(1), point_to_show(2));
glPopName();
}
void Vertex::draw_without_color()
{
if (!visible_)
{
return;
}
glNormal3f( normal_(0), normal_(1), normal_(2));
glVertex3f(position_(0), position_(1), position_(2));
}
void Vertex::draw_without_color( const Matrix44& adjust_matrix)
{
if (!visible_)
{
return;
}
glNormal3f( normal_(0), normal_(1), normal_(2));
Vec4 tmp(position_(0), position_(1), position_(2),1.);
Vec4 point_to_show = adjust_matrix * tmp;
glVertex3f( point_to_show(0), point_to_show(1), point_to_show(2) );
}
void Vertex::draw_with_label( const Matrix44& adjust_matrix )
{
if (!visible_)
{
return;
}
static const IndexType color_step = 47;
IndexType label_color = (label_ * color_step) % 255;
ColorType color = Color_Utility::color_from_table(label_color);
glColor4f( color(0),color(1),color(2),color(3) );
glNormal3f( normal_(0), normal_(1), normal_(2));
Vec4 tmp(position_(0), position_(1), position_(2),1.);
Vec4 point_to_show = adjust_matrix * tmp;
glVertex3f( point_to_show(0), point_to_show(1), point_to_show(2) );
}
void Vertex::draw_with_label( const Matrix44& adjust_matrix, const Vec3& bias )
{
if (!visible_)
{
return;
}
//static const IndexType color_step = 47;
//IndexType label_color = (label_ * color_step) % 255;
//ColorType color = Color_Utility::color_from_table(label_color);
ColorType color = Color_Utility::span_color_from_table(label_);//可视化标签 7-28
//ColorType color = Color_Utility::color_map_one(val_);//可视化曲率
glColor4f( color(0),color(1),color(2),color(3) );
glNormal3f( normal_(0), normal_(1), normal_(2));
Vec4 tmp(position_(0), position_(1), position_(2),1.);
Vec4 point_to_show = adjust_matrix * tmp;
glVertex3f( point_to_show(0)+bias(0), point_to_show(1)+bias(1), point_to_show(2)+bias(2) );
}
void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
{
if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) )
{
try
{
// update the pose
listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map);
listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map);
tf::Vector3 position_( kinect2map.getOrigin() );
position.x() = position_.x();
position.y() = position_.y();
position.z() = position_.z();
tf::Quaternion orientation_( kinect2map.getRotation() );
orientation.x() = orientation_.x();
orientation.y() = orientation_.y();
orientation.z() = orientation_.z();
orientation.w() = orientation_.w();
ROS_INFO_STREAM("position = " << position.transpose() );
ROS_INFO_STREAM("orientation = " << orientation.transpose() );
// update the cloud
pcl::copyPointCloud(*cloud, *xyz_cld_ptr); // Do I need to copy it?
//xyz_cld_ptr = cloud;
cloud_updated = true;
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
}
}
inline cv::Point3d get_cvposition()
{ cv::Point3d cvpos(position_(0), position_(1), position_(2)); return cvpos; }