OSG_BEGIN_NAMESPACE
void drawPhysicsBodyCoordinateSystem(const PhysicsBodyUnrecPtr body, Real32 Length)
{
Pnt3f origin(0.0f,0.0f,0.0f);
Pnt3f x_axis(Length,0.0f,0.0f),
y_axis(0.0f,Length,0.0f),
z_axis(0.0f,0.0f,Length);
//Transform by the bodies position and rotation
Matrix m(body->getTransformation());
m.mult(origin,origin);
m.mult(x_axis,x_axis);
m.mult(y_axis,y_axis);
m.mult(z_axis,z_axis);
glBegin(GL_LINES);
//X Axis
glColor3f(1.0,0.0,0.0);
glVertex3fv(origin.getValues());
glVertex3fv(x_axis.getValues());
//Y Axis
glColor3f(0.0,1.0,0.0);
glVertex3fv(origin.getValues());
glVertex3fv(y_axis.getValues());
//Z Axis
glColor3f(0.0,0.0,1.0);
glVertex3fv(origin.getValues());
glVertex3fv(z_axis.getValues());
glEnd();
}
const LLMatrix3& LLMatrix3::orthogonalize()
{
LLVector3 x_axis(mMatrix[VX]);
LLVector3 y_axis(mMatrix[VY]);
LLVector3 z_axis(mMatrix[VZ]);
x_axis.normVec();
y_axis -= x_axis * (x_axis * y_axis);
y_axis.normVec();
z_axis = x_axis % y_axis;
setRows(x_axis, y_axis, z_axis);
return (*this);
}
Vector getFrameNormal(const door_msgs::Door& door)
{
// normal on frame
Vector p12(door.frame_p1.x-door.frame_p2.x, door.frame_p1.y-door.frame_p2.y, door.frame_p1.z-door.frame_p2.z);
p12.Normalize();
Vector z_axis(0,0,1);
Vector normal = p12 * z_axis;
// make normal point in direction we travel through door
Vector dir(door.travel_dir.x, door.travel_dir.y, door.travel_dir.z);
if (dot(dir, normal) < 0)
normal = normal * -1.0;
return normal;
}
void
pcl::visualization::PCLVisualizerInteractorStyle::setCameraParameters (const Eigen::Matrix3f &intrinsics,
const Eigen::Matrix4f &extrinsics,
int viewport)
{
// Position = extrinsic translation
Eigen::Vector3f pos_vec = extrinsics.block<3, 1> (0, 3);
// Rotate the view vector
Eigen::Matrix3f rotation = extrinsics.block<3, 3> (0, 0);
Eigen::Vector3f y_axis (0.f, 1.f, 0.f);
Eigen::Vector3f up_vec (rotation * y_axis);
// Compute the new focal point
Eigen::Vector3f z_axis (0.f, 0.f, 1.f);
Eigen::Vector3f focal_vec = pos_vec + rotation * z_axis;
// Get the width and height of the image - assume the calibrated centers are at the center of the image
Eigen::Vector2i window_size;
window_size[0] = static_cast<int> (intrinsics (0, 2));
window_size[1] = static_cast<int> (intrinsics (1, 2));
// Compute the vertical field of view based on the focal length and image heigh
double fovy = 2 * atan (window_size[1] / (2. * intrinsics (1, 1))) * 180.0 / M_PI;
rens_->InitTraversal ();
vtkRenderer* renderer = NULL;
int i = 0;
while ((renderer = rens_->GetNextItem ()) != NULL)
{
// Modify all renderer's cameras
if (viewport == 0 || viewport == i)
{
vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
cam->SetPosition (pos_vec[0], pos_vec[1], pos_vec[2]);
cam->SetFocalPoint (focal_vec[0], focal_vec[1], focal_vec[2]);
cam->SetViewUp (up_vec[0], up_vec[1], up_vec[2]);
cam->SetUseHorizontalViewAngle (0);
cam->SetViewAngle (fovy);
cam->SetClippingRange (0.01, 1000.01);
win_->SetSize (window_size[0], window_size[1]);
}
++i;
}
win_->Render ();
}
void Hand_filter::align_quaternion_axis(tf::Quaternion& q_out, float &angle, const tf::Quaternion &q_in, tf::Vector3 target){
tf::Matrix3x3 R; R.setRotation(q_in);
tf::Vector3 z_axis(R[0][2],R[1][2],R[2][2]);
z_axis = z_axis.normalize();
tf::Vector3 c = z_axis.cross(target);
angle = z_axis.dot(target);
tf::Quaternion q_r;
q_r.setX(c.x());
q_r.setY(c.y());
q_r.setZ(c.z());
q_r.setW(sqrt(z_axis.length2() * target.length2()) + angle);
q_out = q_r*q_in;
}
void renderer::Render(void) {
const int N_repeat = 13;
glm::vec3 x_axis(1.0, 0.0, 0.0);
glm::vec3 y_axis(0.0, 1.0, 0.0);
glm::vec3 z_axis(0.0, 0.0, 1.0);
glm::vec3 cam_pos(0, 0, 0);
glm::vec3 cam_up = y_axis;
glm::vec3 cam_right = x_axis;
glm::vec3 cam_front = -z_axis; //oikeakatinen koordinaatisto
glm::mat4 P = glm::lookAt(cam_pos, cam_pos + cam_front, cam_up);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glUseProgram(programID);
glBindVertexArray(VertexArrayID);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, Texture);
// Set our "myTextureSampler" sampler to user Texture Unit 0
glUniform1i(TextureID, 0);
int width, height;
glfwGetFramebufferSize(window, &width, &height);
glm::mat4 V = glm::ortho(-1.0f, 1.0f, -1.0f*height / width, 1.0f*height / width);
glm::mat4 VP = V*P;
for (int j = 0; j < N_repeat; j++) {
glm::mat4 M =
glm::rotate(alpha + j*2.0f*3.14159265f / N_repeat, z_axis)*
glm::translate(glm::vec3(0.0, 0.3, 0));
//glm::scale(glm::vec3(0.75));
MVP = VP*M;
glUniformMatrix4fv(MVP_MatrixID, 1, GL_FALSE, &MVP[0][0]);
glVertexAttrib3f(VERTEX_POSITION, 0.0f, 0.0f, 0.0f);
glPointSize(width / 15);
glDrawArrays(GL_POINTS, 0, 12);
}
glfwSwapBuffers(window);
alpha += 0.005f;
}
void TransformCloud() {
/// get the original point, X and Y direction normal of the new coordinate
Eigen::Affine3f transform;
Eigen::Vector3f y_direction, z_axis, origin;
origin(0) = picked_points[0].x;
origin(1) = picked_points[0].y;
origin(2) = picked_points[0].z;
y_direction(0) = picked_points[2].x - picked_points[1].x;
y_direction(1) = picked_points[2].y - picked_points[1].y;
y_direction(2) = picked_points[2].z - picked_points[1].z;
z_axis(0) = build_plane_coeff(0);
z_axis(1) = build_plane_coeff(1);
z_axis(2) = build_plane_coeff(2);
y_direction.normalize();
z_axis.normalize();
double result2 = y_direction(0) * z_axis(0) + y_direction(1) * z_axis(1)
+ y_direction(2) * z_axis(2);
cout << "Test orthogonal: " << result2 << endl;
// Get transformation matrix
pcl::getTransformationFromTwoUnitVectorsAndOrigin(y_direction,
z_axis, origin, transform);
// transform the cloud
pcl::transformPointCloud(*build_cloud_accurate_plane,
*build_cloud_transformed_plane, transform);
pcl::transformPointCloud(*build_cloud_raw,
*build_cloud_transformed_raw, transform);
// save...
pcl::io::savePCDFileASCII(cmd_arguments.pt_pcd_, *build_cloud_transformed_plane);
cout << cmd_arguments.pt_pcd_ << " is saved.\n";
pcl::io::savePCDFileASCII(cmd_arguments.rt_pcd_, *build_cloud_transformed_raw);
cout << cmd_arguments.rt_pcd_ << " is saved.\n";
viewer->updatePointCloud(build_cloud_transformed_plane, "build plane cloud");
viewer->initCameraParameters();
viewer->removeAllShapes();
// get the bounder box of the transformed cloud
Create_building_plane_bounder_box_and_add_to_viewer();
}
osg::Geometry* NaturalCubicSpline::drawTangentCoordinateSystems()
{
osg::Vec4 vec; // Ortsvektor
osg::Vec4 x_axis(1,0,0,1);
osg::Vec4 y_axis(0,1,0,1);
osg::Vec4 z_axis(0,0,1,1);
osg::Matrix mat;
osg::ref_ptr<osg::Vec4Array>tangent_vertices = new osg::Vec4Array;
for(int i = 0; i < _vertices->getNumElements(); i++)
{
vec = osg::Vec4(
(*_vertices)[i].x(),
(*_vertices)[i].y(),
(*_vertices)[i].z(),
1
);
mat = _matrices[i];
tangent_vertices->push_back(vec);
tangent_vertices->push_back(mat*x_axis);
tangent_vertices->push_back(vec);
tangent_vertices->push_back(mat*y_axis);
tangent_vertices->push_back(vec);
tangent_vertices->push_back(mat*z_axis);
}
osg::ref_ptr<osg::Geometry> tangent_geo = new osg::Geometry;
tangent_geo->setVertexArray( tangent_vertices );
tangent_geo->addPrimitiveSet(
new osg::DrawArrays(GL_LINES, 0, tangent_vertices->getNumElements()) );
return tangent_geo.release();
}
static int moment_of_inertia_features(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& inputCluster) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// Get cloud from publisher
pcl::copyPointCloud(*inputCluster, *cloud);
std::cout << "Hello1" << std::endl; //*
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
std::cout << "Hello2" << std::endl; //*
feature_extractor.setInputCloud (cloud);
std::cout << "Hello3" << std::endl; //*
feature_extractor.compute ();
std::cout << "Hello4" << std::endl; //*
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);
std::cout << "Hello5" << std::endl; //*
feature_extractor.getEccentricity (eccentricity);
std::cout << "Hello6" << std::endl; //*
feature_extractor.getAABB (min_point_AABB, max_point_AABB);
std::cout << "Hello7" << std::endl; //*
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
std::cout << "Hello8" << std::endl; //*
feature_extractor.getEigenValues (major_value, middle_value, minor_value);
std::cout << "Hello9" << std::endl; //*
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
std::cout << "Hello10" << std::endl; //*
feature_extractor.getMassCenter (mass_center);
std::cout << "Hello11" << std::endl; //*
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
while(!viewer->wasStopped()) {
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
//----------------------------------------------
// Draw the scene. A box on the end of the wand
//----------------------------------------------
void sonixApp::myDraw()
{
glMatrixMode(GL_MODELVIEW);
// -- Draw box on wand --- //
gmtl::Matrix44f wandMatrix = mWand->getData(); // Get the wand matrix
glPushMatrix();
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Push the wand matrix on the stack
float wand_color[3];
wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;
if(mButton0->getData() == gadget::Digital::ON)
{
wand_color[0] += 0.5f;
}
if(mButton1->getData() == gadget::Digital::ON)
{
wand_color[1] += 0.5f;
}
if(mButton2->getData() == gadget::Digital::ON)
{
wand_color[2] += 0.5f;
}
if(mButton3->getData() == gadget::Digital::ON)
{
wand_color[0] += 0.5f;
}
if(mButton4->getData() == gadget::Digital::ON)
{
wand_color[1] += 0.5f;
}
if(mButton5->getData() == gadget::Digital::ON)
{
wand_color[2] += 0.5f;
}
glColor3fv(wand_color);
glScalef(0.25f, 0.25f, 0.25f);
drawCube();
glPopMatrix();
// A little laser pointer
glLineWidth(5.0f);
// Draw Axis
glDisable(GL_LIGHTING);
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Goto wand position
gmtl::Vec3f x_axis(7.0f, 0.0f, 0.0f);
gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(x_axis.mData);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(y_axis.mData);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3fv(origin.mData);
glVertex3fv(z_axis.mData);
glEnd();
glPopMatrix();
glEnable(GL_LIGHTING);
glPopMatrix();
}
int main( void ) {
//------------------------------------------------------
//Ex 23 GLM:llä matriisi laskemista
//glm::mat4 matrix1(1.0f, 0.0f, 2.0f, 2.0f,
// 0.0f, 1.0f, 0.0f, 0.0f,
// 1.0f, 1.0f, 1.0f, 2.0f,
// 0.0f, 0.0f, 1.0f, 0.0f);
//glm::mat4 matrix2(0.0f, 0.0f, 0.0f, 2.0f,
// 1.0f, 1.0f, 0.0f, 0.0f,
// 1.0f, 1.0f, 0.0f, 2.0f,
// 0.0f, 0.0f, 1.0f, 0.0f);
//glm::vec4 multiplyVector(3.0f, 4.0f, -2.0f, 1.0f);
//glm::vec4 xTulos;
//xTulos = (matrix1 * matrix2) * multiplyVector;
///*for (int j = 0; j < sizeof(xTulos); j++)
//{
// std::cout << xTulos[j] << std::endl;
//}*/
//std::cout << xTulos[0] << ", " << xTulos[1] << ", " << xTulos[2] << ", " << xTulos[3] << std::endl;
//--------------------------------------------------------
//Test
//------------------------------------------------------
std::cout << "Ex 25-------------------------------------------------------\n" << std::endl;
glm::vec4 P1(0.0f, 0.0f, 0.0f, 1.0f);
glm::vec4 P2(1.0f, 0.0f, 0.0f, 1.0f);
glm::vec4 P3(0.0f, 1.0f, 0.0f, 1.0f);
glm::vec3 x_axis(1.0f, 0.0f, 0.0f);
glm::vec3 y_axis(0.0f, 1.0f, 0.0f);
glm::vec3 z_axis(0.0f, 0.0f, 1.0f);
glm::vec3 s(0.3f); //skaalaus
glm::vec3 t(-2.0f, -1.0f, 0.0f); //siirto
glm::vec3 r = z_axis; //kierto
glm::mat4 R = rotate(3.14159265f / 6.0f, r);
glm::mat4 S = scale(s);
glm::mat4 T = translate(t);
glm::mat4 T_total = T*S*R;
PrintVec(T_total*P1);
PrintVec(T_total*P2);
PrintVec(T_total*P3);
PrintMatrix(T_total);
std::cout << "\nEx 26--------------------------------------------------------" << std::endl;
glm::vec3 cam_pos(1.2f, 0.1f, 0.0);
glm::vec3 cam_up = y_axis;
glm::vec3 cam_right = x_axis;
glm::vec3 cam_front = z_axis; //oikeakätinen koordinaatisto
glm::mat4 C = lookAt(cam_pos, cam_pos + cam_front, cam_up);
T_total = C*T_total;
PrintVec(T_total*P1);
PrintVec(T_total*P2);
PrintVec(T_total*P3);
PrintMatrix(T_total);
std::cout << "\nEx 27---------------------------------------------------------" << std::endl;
float v_width = 6.0; //viewport in camera coord
float v_height = 6.0;
glm::mat4 T_projection = glm::ortho(-0.5f*v_width, 0.5f*v_width,
-0.5f*v_height, 0.5f*v_height);
T_total = T_projection*T_total;
PrintVec(T_total*P1);
PrintVec(T_total*P2);
PrintVec(T_total*P3);
PrintMatrix(T_total);
//------------------------------------------------------
std::cout << "\n Ex 30--------------------------------------------------------" << std::endl;
if (!glfwInit())
{
fprintf(stderr, "Failed to initialize GLFW\n");
return -1;
}
glfwWindowHint(GLFW_SAMPLES, 4);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
int w = 1024;
int h = 768;
window = glfwCreateWindow(w, h,
"Tutorial 02 - Red triangle", NULL, NULL);
if (window == NULL){
fprintf(stderr, "Failed to open GLFW window.");
glfwTerminate();
return -1;
}
glfwMakeContextCurrent(window);
glfwSetFramebufferSizeCallback(window, renderer::FramebufferSizeCallback);
renderer::FramebufferSizeCallback(window, w, h);
glewExperimental = true; // Needed for core profile
if (glewInit() != GLEW_OK) {
fprintf(stderr, "Failed to initialize GLEW\n");
return -1;
}
glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);
renderer::Init(window);
do{
renderer::Render();
glfwPollEvents();
} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
glfwWindowShouldClose(window) == 0);
renderer::Uninit();
glfwTerminate();
//------------------------------------------------------
// TESTI KOLMIO
//if (!glfwInit())
//{
// fprintf(stderr, "Failed to initialize GLFW\n");
// return -1;
//}
//glfwWindowHint(GLFW_SAMPLES, 4);
//glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
//glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
//glfwWindowHint(GLFW_OPENGL_PROFILE,
// GLFW_OPENGL_CORE_PROFILE);
//window = glfwCreateWindow(1024, 768,
// "Tutorial 02 - Red triangle", NULL, NULL);
//if (window == NULL){
// fprintf(stderr, "Failed to open GLFW window.");
// glfwTerminate();
// return -1;
//}
//glfwMakeContextCurrent(window);
//glewExperimental = true; // Needed for core profile
//if (glewInit() != GLEW_OK) {
// fprintf(stderr, "Failed to initialize GLEW\n");
// return -1;
//}
//glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);
//Init();
//do{
// Render();
// glfwPollEvents();
//} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
// glfwWindowShouldClose(window) == 0);
//glfwTerminate();
//---------------------------------------------------------
system("pause");
return 0;
}
// Utility function to test intersection between ray and mesh bounding box
bool test_ray_oobb(const glm::vec3 &origin, const glm::vec3 &direction, const glm::vec3 &aabb_min,
const glm::vec3 &aabb_max, const glm::mat4 &model, float &distance) {
float t_min = 0.0f;
float t_max = 100000.0f;
// Calculate OOBB position in world space
glm::vec3 OOBB_pos_world(model[3].x, model[3].y, model[3].z);
// Calculate direction from ray origin to OOBB position
glm::vec3 delta = OOBB_pos_world - origin;
// Test intersection with the 2 planes perpendicular to the OOBB x-axis
{
// Get x-axis of transformed object
glm::vec3 x_axis(model[0].x, model[0].y, model[0].z);
x_axis = glm::normalize(x_axis);
// Calculate the cosine of the x_axis and delta
float e = glm::dot(x_axis, delta);
// Calculate the cosine of the ray direction and x_axis
float f = glm::dot(direction, x_axis);
// Test if ray and x_axis are parallel
if (fabs(f) > 0.001f) {
// Calculate intersection distance with the left plane
float t1 = (e + aabb_min.x) / f;
// Calculate intersection distance with the right plane
float t2 = (e + aabb_max.x) / f;
// t1 needs to be the nearest intersection
if (t1 > t2)
std::swap(t1, t2);
// t_max is the nearest far intersection
t_max = glm::min(t_max, t2);
// t_min is the farthese near intersection
t_min = glm::max(t_min, t1);
// If far is closer than near there is no intersection on this axis
if (t_max < t_min)
return false;
}
// ray and x_axis is almost parallel
else if (-e + aabb_min.x > 0.0f || -e + aabb_max.x < 0.0f)
return false;
}
// Test intersection with the 2 planes perpendicular to the OOBB y-axis
{
// Get y-axis of transformed object
glm::vec3 y_axis(model[1].x, model[1].y, model[1].z);
y_axis = glm::normalize(y_axis);
// Calculate the cosine of the y_axis and delta
float e = glm::dot(y_axis, delta);
// Calculate the cosine of the ray direction and y_axis
float f = glm::dot(direction, y_axis);
// Test if ray and y_axis are parallel
if (fabs(f) > 0.001f) {
// Calculate intersection distance with the left plane
float t1 = (e + aabb_min.y) / f;
// Calculate intersection distance with the right plane
float t2 = (e + aabb_max.y) / f;
// t1 needs to be the nearest intersection
if (t1 > t2)
std::swap(t1, t2);
// t_max is the nearest far intersection
t_max = glm::min(t_max, t2);
// t_min is the farthese near intersection
t_min = glm::max(t_min, t1);
// If far is closer than near there is no intersection on this axis
if (t_max < t_min)
return false;
}
// ray and y_axis is almost parallel
else if (-e + aabb_min.y > 0.0f || -e + aabb_max.y < 0.0f)
return false;
}
// Test intersection with the 2 planes perpendicular to the OOBB z-axis
{
// Get x-axis of transformed object
glm::vec3 z_axis(model[2].x, model[2].y, model[2].z);
z_axis = glm::normalize(z_axis);
// Calculate the cosine of the z_axis and delta
float e = glm::dot(z_axis, delta);
// Calculate the cosine of the ray direction and z_axis
float f = glm::dot(direction, z_axis);
// Test if ray and z_axis are parallel
if (fabs(f) > 0.001f) {
// Calculate intersection distance with the left plane
float t1 = (e + aabb_min.z) / f;
// Calculate intersection distance with the right plane
float t2 = (e + aabb_max.z) / f;
// t1 needs to be the nearest intersection
if (t1 > t2)
std::swap(t1, t2);
// t_max is the nearest far intersection
t_max = glm::min(t_max, t2);
// t_min is the farthese near intersection
t_min = glm::max(t_min, t1);
// If far is closer than near there is no intersection on this axis
if (t_max < t_min)
return false;
}
// ray and z_axis is almost parallel
else if (-e + aabb_min.z > 0.0f || -e + aabb_max.z < 0.0f)
return false;
}
// Set distance and return true
distance = t_min;
return true;
}
int main (int argc, char** argv)
{
if (argc != 3)
return (0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (pcl::io::loadPLYFile (argv[1], *cloud) == -1)
return (-1);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::io::loadPLYFile(argv[2], *cloud2);
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);
feature_extractor.getEccentricity (eccentricity);
feature_extractor.getAABB (min_point_AABB, max_point_AABB);
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
feature_extractor.getEigenValues (major_value, middle_value, minor_value);
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
feature_extractor.getMassCenter (mass_center);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (2.0, 0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud");
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
while(!viewer->wasStopped())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
void GraphManager::visualizeGraphNodes() const {
std::clock_t starttime=std::clock();
if (marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking
visualization_msgs::Marker nodes_marker;
nodes_marker.header.frame_id = "/openni_rgb_optical_frame"; //TODO: Should be a meaningfull fixed frame with known relative pose to the camera
nodes_marker.header.stamp = ros::Time::now();
nodes_marker.ns = "camera_pose_graph"; // Set the namespace and id for this marker. This serves to create a unique ID
nodes_marker.id = 1; // Any marker sent with the same namespace and id will overwrite the old one
nodes_marker.type = visualization_msgs::Marker::LINE_LIST;
nodes_marker.action = visualization_msgs::Marker::ADD; // Set the marker action. Options are ADD and DELETE
nodes_marker.frame_locked = true; //rviz automatically retransforms the markers into the frame every update cycle
// Set the scale of the marker -- 1x1x1 here means 1m on a side
nodes_marker.scale.x = 0.002;
//Global pose (used to transform all points) //TODO: is this the default pose anyway?
nodes_marker.pose.position.x = 0;
nodes_marker.pose.position.y = 0;
nodes_marker.pose.position.z = 0;
nodes_marker.pose.orientation.x = 0.0;
nodes_marker.pose.orientation.y = 0.0;
nodes_marker.pose.orientation.z = 0.0;
nodes_marker.pose.orientation.w = 1.0;
// Set the color -- be sure to set alpha to something non-zero!
nodes_marker.color.r = 1.0f;
nodes_marker.color.g = 0.0f;
nodes_marker.color.b = 0.0f;
nodes_marker.color.a = 1.0f;
geometry_msgs::Point tail; //same startpoint for each line segment
geometry_msgs::Point tip; //different endpoint for each line segment
std_msgs::ColorRGBA arrow_color_red ; //red x axis
arrow_color_red.r = 1.0;
arrow_color_red.a = 1.0;
std_msgs::ColorRGBA arrow_color_green; //green y axis
arrow_color_green.g = 1.0;
arrow_color_green.a = 1.0;
std_msgs::ColorRGBA arrow_color_blue ; //blue z axis
arrow_color_blue.b = 1.0;
arrow_color_blue.a = 1.0;
Vector3f origin(0.0,0.0,0.0);
Vector3f x_axis(0.2,0.0,0.0); //20cm long axis for the first (almost fixed) node
Vector3f y_axis(0.0,0.2,0.0);
Vector3f z_axis(0.0,0.0,0.2);
Vector3f tmp; //the transformed endpoints
int counter = 0;
AISNavigation::PoseGraph3D::Vertex* v; //used in loop
AISNavigation::PoseGraph3D::VertexIDMap::iterator vertex_iter = optimizer_->vertices().begin();
for(/*see above*/; vertex_iter != optimizer_->vertices().end(); vertex_iter++, counter++) {
v = static_cast<AISNavigation::PoseGraph3D::Vertex*>((*vertex_iter).second);
//v->transformation.rotation().x()+ v->transformation.rotation().y()+ v->transformation.rotation().z()+ v->transformation.rotation().w();
tmp = v->transformation * origin;
tail.x = tmp.x();
tail.y = tmp.y();
tail.z = tmp.z();
//Endpoints X-Axis
nodes_marker.points.push_back(tail);
nodes_marker.colors.push_back(arrow_color_red);
tmp = v->transformation * x_axis;
tip.x = tmp.x();
tip.y = tmp.y();
tip.z = tmp.z();
nodes_marker.points.push_back(tip);
nodes_marker.colors.push_back(arrow_color_red);
//Endpoints Y-Axis
nodes_marker.points.push_back(tail);
nodes_marker.colors.push_back(arrow_color_green);
tmp = v->transformation * y_axis;
tip.x = tmp.x();
tip.y = tmp.y();
tip.z = tmp.z();
nodes_marker.points.push_back(tip);
nodes_marker.colors.push_back(arrow_color_green);
//Endpoints Z-Axis
nodes_marker.points.push_back(tail);
nodes_marker.colors.push_back(arrow_color_blue);
tmp = v->transformation * z_axis;
tip.x = tmp.x();
tip.y = tmp.y();
tip.z = tmp.z();
nodes_marker.points.push_back(tip);
nodes_marker.colors.push_back(arrow_color_blue);
//shorten all nodes after the first one
x_axis.x() = 0.1;
y_axis.y() = 0.1;
z_axis.z() = 0.1;
}
marker_pub_.publish (nodes_marker);
ROS_INFO("published %d graph nodes", counter);
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
void soundManagerApp::myDraw()
{
//std::cout << "\n--- myDraw() ---\n";
//std::cout << "HeadPos:" << vrj::Coord(*mHead->getData()).pos << "\t"
// << "WandPos:" << vrj::Coord(*mWand->getData()).pos << std::endl;
glMatrixMode(GL_MODELVIEW);
// -- Draw box on wand --- //
gmtl::Matrix44f wandMatrix = mWand->getData(); // Get the wand matrix
glPushMatrix();
// cout << "wand:\n" << *wandMatrix << endl;
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Push the wand matrix on the stack
//glColor3f(1.0f, 0.0f, 1.0f);
float wand_color[3];
wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;
if(mButton0->getData() == gadget::Digital::ON)
wand_color[0] += 0.5f;
if(mButton1->getData() == gadget::Digital::ON)
wand_color[1] += 0.5f;
if(mButton2->getData() == gadget::Digital::ON)
wand_color[2] += 0.5f;
if(mButton3->getData() == gadget::Digital::ON)
wand_color[0] += 0.5f;
if(mButton4->getData() == gadget::Digital::ON)
wand_color[1] += 0.5f;
if(mButton5->getData() == gadget::Digital::ON)
wand_color[2] += 0.5f;
glColor3fv(wand_color);
glScalef(0.25f, 0.25f, 0.25f);
drawCube();
glPopMatrix();
// A little laser pointer
glLineWidth(5.0f);
// Draw Axis
glDisable(GL_LIGHTING);
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Goto wand position
gmtl::Vec3f x_axis(7.0f,0.0f,0.0f);
gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(x_axis.mData);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(y_axis.mData);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3fv(origin.mData);
glVertex3fv(z_axis.mData);
glEnd();
glPopMatrix();
glEnable(GL_LIGHTING);
glPopMatrix();
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
ne.setInputCloud (cloud);
cv::Mat input_cloud;
input_cloud = cv::Mat(480, 640, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<cloud->height; h++) {
for (int w=0; w<cloud->width; w++) {
pcl::PointXYZRGBA point = cloud->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
int x_pos = 320 + point.x/point.z *480;
int y_pos = 240 + point.y/point.z * 480;
//std::cout << "x pos" << x_pos << "y pos" << y_pos << std::endl;
if( x_pos > 0 and x_pos < 640 and y_pos > 0 and y_pos < 480){
input_cloud.at<cv::Vec3b>(y_pos,x_pos)[0] = rgb[2];
input_cloud.at<cv::Vec3b>(y_pos,x_pos)[1] = rgb[1];
input_cloud.at<cv::Vec3b>(y_pos,x_pos)[2] = rgb[0];
}
}
}
}
std::cout << " saving input image " << std::endl;
cv::imwrite( "image_no_pespective_correction.png", input_cloud );
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
ne.setSearchMethod (tree);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
std::cout << " starting computation " << std::endl;
// Compute the features
ne.compute (*cloud_normals);
std::cout << "cloud_normals->points.size (): " << cloud_normals->points.size () << std::endl;
pcl::Normal normal_point = cloud_normals->at(320, 240);
pcl::PointXYZRGBA _point = cloud->at(320, 240);
std::cout << "cloud_normal at point: " << normal_point << std::endl;
std::cout << "z of cloud at point : " << _point.z << std::endl;
Eigen::Vector3f z_axis(normal_point.normal_x,normal_point.normal_y,normal_point.normal_z);
Eigen::Vector3f x_axis_standard(1,0,0);
Eigen::Vector3f x_proj = x_axis_standard - x_axis_standard.dot(z_axis)*z_axis;
Eigen::Affine3f translate_z;
Eigen::Affine3f transformation;
Eigen::Vector3f tmp0 = x_proj.normalized();
Eigen::Vector3f tmp1 = (z_axis.normalized().cross(x_proj.normalized())).normalized();
Eigen::Vector3f tmp2 = z_axis.normalized();
translate_z(0,0)=1; translate_z(0,1)=0; translate_z(0,2)=0; translate_z(0,3)=0.0f;
translate_z(1,0)=0; translate_z(1,1)=1; translate_z(1,2)=0; translate_z(1,3)=0.0f;
translate_z(2,0)=0; translate_z(2,1)=0; translate_z(2,2)=1; translate_z(2,3)=-_point.z;
translate_z(3,0)=0.0f; translate_z(3,1)=0.0f; translate_z(3,2)=0.0f; translate_z(3,3)=1.0f;
transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud1 (new pcl::PointCloud<pcl::PointXYZRGBA>);
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*cloud, *transformed_cloud1, translate_z);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*transformed_cloud1, *transformed_cloud2, transformation);
translate_z(2,3)=_point.z;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud3 (new pcl::PointCloud<pcl::PointXYZRGBA>);
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*transformed_cloud2, *transformed_cloud3, translate_z);
_point = transformed_cloud3->at(320, 240);
std::cout << "cloud at point: " << _point << std::endl;
cv::Mat output_cloud;
output_cloud = cv::Mat(480, 640, CV_8UC3, cv::Scalar(0,0,255) );
cv::Mat distance;
distance = cv::Mat(480, 640, CV_32FC1, -1.2); //::zeros
std::cout << "depth " << distance.at<float>(0,0) << std::endl;
if (!transformed_cloud3->empty())
{
for (int h=0; h<transformed_cloud3->height; h++)
{
for (int w=0; w<transformed_cloud3->width; w++)
{
pcl::PointXYZRGBA point = transformed_cloud3->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
int x_pos = 320 - point.x/point.z * 480;
int y_pos = 240 + point.y/point.z * 480;
if( x_pos > 0 and x_pos < 640 and y_pos > 0 and y_pos < 480)
{
if( point.z > distance.at<float>(y_pos,x_pos) and point.z < -0.8 )
{
output_cloud.at<cv::Vec3b>(y_pos,x_pos)[0] = rgb[2];
output_cloud.at<cv::Vec3b>(y_pos,x_pos)[1] = rgb[1];
output_cloud.at<cv::Vec3b>(y_pos,x_pos)[2] = rgb[0];
distance.at<float>(y_pos,x_pos) = point.z;
}
}
}
}
}
std::cout << " saving output image " << std::endl;
cv::imwrite( "transformed_image_no_interpolation.png", output_cloud );
//cv::Mat input_cloud;
input_cloud = cv::Mat(480, 640, CV_8UC3);
for (int h=0; h< output_cloud.rows ; h++)
{
for (int w=0; w< output_cloud.cols ; w++)
{
cv::Vec3b color = output_cloud.at<cv::Vec3b>(cv::Point(w,h));
if( !( (int)output_cloud.at<cv::Vec3b>(h, w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, w)[2] == 255) )
{
input_cloud.at<cv::Vec3b>(h, w)[0] = output_cloud.at<cv::Vec3b>(h, w)[0];
input_cloud.at<cv::Vec3b>(h, w)[1] = output_cloud.at<cv::Vec3b>(h, w)[1];
input_cloud.at<cv::Vec3b>(h, w)[2] = output_cloud.at<cv::Vec3b>(h, w)[2];
}
else
{
// std::cout << " oh god" << std::endl;
int left_pos = - 1;
int right_pos = 1;
while( left_pos + w > 0
and ( (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[2] == 255) )
{
//std::cout << "ha" << std::endl;
left_pos--;
}
while( right_pos + w < 640 -1
and ( (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[2] == 255) )
{
//std::cout << "ha" << std::endl;
right_pos++;
}
if( left_pos + w >= 0 and right_pos + w < 640)
{
if( !( (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[2] == 255)
and !( (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[2] == 255) )
{
float left_right_weight = -left_pos/right_pos;
int pixel_color = (int) output_cloud.at<cv::Vec3b>(h, left_pos + w)[0]/2 + output_cloud.at<cv::Vec3b>(h, right_pos + w)[0]/2 ;
input_cloud.at<cv::Vec3b>(h, w)[0] = pixel_color;
input_cloud.at<cv::Vec3b>(h, w)[1] = pixel_color;
input_cloud.at<cv::Vec3b>(h, w)[2] = pixel_color;
}
else
{
input_cloud.at<cv::Vec3b>(h, w)[0] = 255;
input_cloud.at<cv::Vec3b>(h, w)[1] = 0;
input_cloud.at<cv::Vec3b>(h, w)[2] = 0;
}
}
}
}
}
std::cout << " saving output image " << std::endl;
cv::imwrite( "transformed_image_perspective.png", input_cloud );
return 0;
}
void wandApp::draw()
{
glClear(GL_DEPTH_BUFFER_BIT);
//std::cout << "\n--- myDraw() ---\n";
// std::cout << "HeadPos:" << gmtl::makeTrans<gmtl::Vec3f>(mHead->getData()) << "\t"
// << "WandPos:" << gmtl::makeTrans<gmtl::Vec3f>(mWand->getData()) << std::endl;
glMatrixMode(GL_MODELVIEW);
// -- Draw box on wand --- //
gmtl::Matrix44f wandMatrix = mWand->getData(); // Get the wand matrix
glPushMatrix();
// cout << "wand:\n" << *wandMatrix << endl;
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Push the wand matrix on the stack
//glColor3f(1.0f, 0.0f, 1.0f);
float wand_color[3];
wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;
if (mButton0->getData() == gadget::Digital::ON)
{
wand_color[0] += 0.5f;
}
if (mButton1->getData() == gadget::Digital::ON)
{
wand_color[1] += 0.5f;
}
if (mButton2->getData() == gadget::Digital::ON)
{
wand_color[2] += 0.5f;
}
if (mButton3->getData() == gadget::Digital::ON)
{
wand_color[0] += 0.5f;
}
if (mButton4->getData() == gadget::Digital::ON)
{
wand_color[1] += 0.5f;
}
if (mButton5->getData() == gadget::Digital::ON)
{
wand_color[2] += 0.5f;
}
glColor3fv(wand_color);
glScalef(0.25f, 0.25f, 0.25f);
drawCube();
glPopMatrix();
// A little laser pointer
glLineWidth(5.0f);
// Draw Axis
glDisable(GL_LIGHTING);
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Goto wand position
gmtl::Vec3f x_axis(7.0f,0.0f,0.0f);
gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(x_axis.mData);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(y_axis.mData);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3fv(origin.mData);
glVertex3fv(z_axis.mData);
glEnd();
glPopMatrix();
glEnable(GL_LIGHTING);
glPopMatrix();
// Draw the head history
glLineWidth(1.0f);
glPushMatrix();
glBegin(GL_LINES);
glColor3f(0.0f, 0.8f, 0.8f);
for(unsigned i=0; i<mHeadHistory.size(); ++i)
{
glVertex3fv(mHeadHistory[i].mData);
}
glEnd();
glPopMatrix();
mPerfProbe.draw();
}
