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();
}
// -------------------------------------------------------------------
void Camera::setWindowImpl() const
{
if (!mWindowSet || !mRecalcWindow)
return;
// Calculate general projection parameters
Real vpLeft, vpRight, vpBottom, vpTop;
calcProjectionParameters(vpLeft, vpRight, vpBottom, vpTop);
Real vpWidth = vpRight - vpLeft;
Real vpHeight = vpTop - vpBottom;
Real wvpLeft = vpLeft + mWLeft * vpWidth;
Real wvpRight = vpLeft + mWRight * vpWidth;
Real wvpTop = vpTop - mWTop * vpHeight;
Real wvpBottom = vpTop - mWBottom * vpHeight;
Vector3 vp_ul (wvpLeft, wvpTop, -mNearDist);
Vector3 vp_ur (wvpRight, wvpTop, -mNearDist);
Vector3 vp_bl (wvpLeft, wvpBottom, -mNearDist);
Vector3 vp_br (wvpRight, wvpBottom, -mNearDist);
Matrix4 inv = mViewMatrix.inverseAffine();
Vector3 vw_ul = inv.transformAffine(vp_ul);
Vector3 vw_ur = inv.transformAffine(vp_ur);
Vector3 vw_bl = inv.transformAffine(vp_bl);
Vector3 vw_br = inv.transformAffine(vp_br);
mWindowClipPlanes.clear();
if (mProjType == PT_PERSPECTIVE)
{
Vector3 position = getPositionForViewUpdate();
mWindowClipPlanes.push_back(Plane(position, vw_bl, vw_ul));
mWindowClipPlanes.push_back(Plane(position, vw_ul, vw_ur));
mWindowClipPlanes.push_back(Plane(position, vw_ur, vw_br));
mWindowClipPlanes.push_back(Plane(position, vw_br, vw_bl));
}
else
{
Vector3 x_axis(inv[0][0], inv[0][1], inv[0][2]);
Vector3 y_axis(inv[1][0], inv[1][1], inv[1][2]);
x_axis.normalise();
y_axis.normalise();
mWindowClipPlanes.push_back(Plane( x_axis, vw_bl));
mWindowClipPlanes.push_back(Plane(-x_axis, vw_ur));
mWindowClipPlanes.push_back(Plane( y_axis, vw_bl));
mWindowClipPlanes.push_back(Plane(-y_axis, vw_ur));
}
mRecalcWindow = false;
}
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);
}
std::vector<TagMatch> detectTags(const cv::Mat& image) {
int residual = image.cols % IMAGE_U8_DEFAULT_ALIGNMENT;
cv::Mat img_aligned;
if (residual != 0) {
cv::copyMakeBorder(image, img_aligned, 0, 0, (IMAGE_U8_DEFAULT_ALIGNMENT - residual) / 2, (IMAGE_U8_DEFAULT_ALIGNMENT - residual) / 2, cv::BORDER_CONSTANT, 0);
} else {
img_aligned = image;
}
cv::cvtColor(img_aligned, img_aligned, CV_RGB2GRAY);
image_u8_t *image_u8 = fromCvMat(img_aligned);
std::vector<TagMatch> tags = detectTags(image_u8);
if (show_window) {
cv::cvtColor(img_aligned, img_aligned, CV_GRAY2RGB);
for (int i = 0; i < tags.size(); i++) {
cv::line(img_aligned, tags[i].p0, tags[i].p1, cv::Scalar(255,0,0), 2, CV_AA);
cv::line(img_aligned, tags[i].p1, tags[i].p2, cv::Scalar(0,255,0), 2, CV_AA);
cv::line(img_aligned, tags[i].p2, tags[i].p3, cv::Scalar(0,0,255), 2, CV_AA);
cv::line(img_aligned, tags[i].p3, tags[i].p0, cv::Scalar(0,0,255), 2, CV_AA);
Eigen::Vector3d x_axis(2,0,1);
Eigen::Vector3d y_axis(0,2,1);
Eigen::Vector3d origin(0,0,1);
Eigen::Vector3d px = tags[i].H * x_axis;
Eigen::Vector3d py = tags[i].H * y_axis;
Eigen::Vector3d o = tags[i].H * origin;
px/= px[2];
py/= py[2];
o/= o[2];
cv::line(img_aligned, cv::Point2d(o[0], o[1]), cv::Point2d(px[0], px[1]), cv::Scalar(255,0,255), 1, CV_AA);
cv::line(img_aligned, cv::Point2d(o[0], o[1]), cv::Point2d(py[0], py[1]), cv::Scalar(255,255,0), 1, CV_AA);
}
cv::imshow("detections", img_aligned);
cv::waitKey(1);
}
image_u8_destroy(image_u8);
return tags;
}
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 Camera::rotate(double dtheta, double dphi) {
glm::vec3 y_axis(0.0, 1.0, 0.0);
glm::vec3 cam_dir = m_position - m_focus_point;
float radius = glm::length(cam_dir);
cam_dir = glm::normalize(cam_dir);
cam_dir = glm::rotate(cam_dir, (float)dtheta, y_axis);
glm::vec3 horizontal = glm::normalize(glm::cross(y_axis, cam_dir));
cam_dir = glm::rotate(cam_dir, (float)dphi, horizontal);
float angle = glm::dot(cam_dir, y_axis);
if (angle > 0.99863 || angle < -0.99863) {
cam_dir = glm::rotate(cam_dir, (float)(-1 * dphi), horizontal);
}
m_position = m_focus_point + cam_dir*radius;
}
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;
}
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();
}
bool FloorFilter::GetFloorLine(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &input_cloud,
const std::vector<int> &indices,
Eigen::ParametrizedLine<float, 3> *line, std::vector<int> *inlier_indices) {
Eigen::ParametrizedLine<float, 3> sensor_floor_intersection_line;
if (!FindSensorPlaneIntersection(input_cloud->header.stamp, &sensor_floor_intersection_line)) {
// It is impossible to find a correct floor line because the
// sensor plane is either parallel to the floor line or intersects
// with it behind the robot.
return false;
}
Eigen::Vector3f y_axis(0, 1, 0);
if (!FindLine(input_cloud, indices, line, inlier_indices)) {
ROS_DEBUG("RANSAC couldn't find a floor line.");
*line = sensor_floor_intersection_line;
}
else if (!geometry::VectorsParallel(line->direction(), y_axis, max_floor_x_rotation_)) {
ROS_DEBUG("The angle between the found floor line and the x-y-plane above threshold."
"Rejecting the line and using the intersection between x-y-plane and sensor plane.");
inlier_indices->clear();
*line = sensor_floor_intersection_line;
}
return true;
}
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();
}
void MapFrame::paintEvent(QPaintEvent* event)
{
// Colorblind friendly colors
QColor green(17, 192, 131);
QColor red(255, 65, 30);
float max_seen_height = -std::numeric_limits<float>::max(); // std::numeric_limits<float>::max() is the max possible floating point value
float max_seen_width = -std::numeric_limits<float>::max();
float min_seen_x = std::numeric_limits<float>::max();
float min_seen_y = std::numeric_limits<float>::max();
// Set the max and min seen values depending on which data the user has selected to view
// Check each of the display data options and choose the most extreme value from those selected by the user
if (display_ekf_data)
{
min_seen_x = min_ekf_seen_x[rover_to_display];
min_seen_y = min_ekf_seen_y[rover_to_display];
max_seen_height = max_ekf_seen_height[rover_to_display];
max_seen_width = max_ekf_seen_height[rover_to_display];
}
if (display_gps_data)
{
if (min_seen_x > min_gps_seen_x[rover_to_display]) min_seen_x = min_gps_seen_x[rover_to_display];
if (min_seen_y > min_gps_seen_y[rover_to_display]) min_seen_y = min_gps_seen_y[rover_to_display];
if (max_seen_height < max_gps_seen_height[rover_to_display]) max_seen_height = max_gps_seen_height[rover_to_display];
if (max_seen_width < max_gps_seen_width[rover_to_display]) max_seen_width = max_gps_seen_width[rover_to_display];
}
if (display_encoder_data)
{
if (min_seen_x > min_encoder_seen_x[rover_to_display]) min_seen_x = min_encoder_seen_x[rover_to_display];
if (min_seen_y > min_encoder_seen_y[rover_to_display]) min_seen_y = min_encoder_seen_y[rover_to_display];
if (max_seen_height < max_encoder_seen_height[rover_to_display]) max_seen_height = max_encoder_seen_height[rover_to_display];
if (max_seen_width < max_encoder_seen_width[rover_to_display]) max_seen_width = max_encoder_seen_width[rover_to_display];
}
// Begin drawing the map
QPainter painter(this);
painter.setPen(Qt::white);
// Track the frames per second for development purposes
QString frames_per_second;
frames_per_second = QString::number(frames /(frame_rate_timer.elapsed() / 1000.0), 'f', 0) + " FPS";
QFontMetrics fm(painter.font());
painter.drawText(this->width()-fm.width(frames_per_second), fm.height(), frames_per_second);
frames++;
if (!(frames % 100)) // time how long it takes to dispay 100 frames
{
frame_rate_timer.start();
frames = 0;
}
// end frames per second
if (ekf_rover_path[rover_to_display].empty() && encoder_rover_path[rover_to_display].empty() && gps_rover_path[rover_to_display].empty() && target_locations[rover_to_display].empty() && collection_points[rover_to_display].empty())
{
painter.drawText(QPoint(50,50), "Map Frame: Nothing to display.");
return;
}
int map_origin_x = fm.width(QString::number(-max_seen_height, 'f', 1)+"m");
int map_origin_y = 2*fm.height();
int map_width = this->width()-map_origin_x;
int map_height = this->height()-map_origin_y;
int map_center_x = map_origin_x+((map_width-map_origin_x)/2);
int map_center_y = map_origin_y+((map_height-map_origin_y)/2);
// Draw the scale bars
//painter.setPen(Qt::gray);
//painter.drawLine(QPoint(map_center_x, map_origin_y), QPoint(map_center_x, map_height));
//painter.drawLine(QPoint(map_origin_x, map_center_y), QPoint(map_width, map_center_y));
//painter.setPen(Qt::white);
// Cross hairs at map display center
QPoint axes_origin(map_origin_x,map_origin_y);
QPoint x_axis(map_width,map_origin_y);
QPoint y_axis(map_origin_x,map_height);
painter.drawLine(axes_origin, x_axis);
painter.drawLine(axes_origin, y_axis);
painter.drawLine(QPoint(map_width, map_origin_y), QPoint(map_width, map_height));
painter.drawLine(QPoint(map_origin_x, map_height), QPoint(map_width, map_height));
// Draw north arrow
QPoint northArrow_point(map_center_x, 0);
QPoint northArrow_left(map_center_x - 5, 5);
QPoint northArrow_right(map_center_x + 5, 5);
QRect northArrow_textBox(northArrow_left.x(), northArrow_left.y(), 10, 15);
painter.drawLine(northArrow_left, northArrow_right);
painter.drawLine(northArrow_left, northArrow_point);
painter.drawLine(northArrow_right, northArrow_point);
painter.drawText(northArrow_textBox, QString("N"));
// Draw rover origin crosshairs
// painter.setPen(green);
// Check encoder has any values in it
if (ekf_rover_path[rover_to_display].empty())
{
painter.drawText(QPoint(50,50), "Map Frame: No encoder data received.");
return;
}
float initial_x = 0.0; //ekf_rover_path[rover_to_display].begin()->first;
float initial_y = 0.001; //ekf_rover_path[rover_to_display].begin()->second;
float rover_origin_x = map_origin_x+((initial_x-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float rover_origin_y = map_origin_y+((initial_y-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
painter.setPen(Qt::gray);
painter.drawLine(QPoint(rover_origin_x, map_origin_y), QPoint(rover_origin_x, map_height));
painter.drawLine(QPoint(map_origin_x, rover_origin_y), QPoint(map_width, rover_origin_y));
painter.setPen(Qt::white);
int n_ticks = 6;
float tick_length = 5;
QPoint x_axis_ticks[n_ticks];
QPoint y_axis_ticks[n_ticks];
for (int i = 0; i < n_ticks-1; i++)
{
x_axis_ticks[i].setX(axes_origin.x()+(i+1)*map_width/n_ticks);
x_axis_ticks[i].setY(axes_origin.y());
y_axis_ticks[i].setX(axes_origin.x());
y_axis_ticks[i].setY(axes_origin.y()+(i+1)*map_height/n_ticks);
}
for (int i = 0; i < n_ticks-1; i++)
{
painter.drawLine(x_axis_ticks[i], QPoint(x_axis_ticks[i].x(), x_axis_ticks[i].y()+tick_length));
painter.drawLine(y_axis_ticks[i], QPoint(y_axis_ticks[i].x()+tick_length, y_axis_ticks[i].y()));
}
for (int i = 0; i < n_ticks-1; i++)
{
float fraction_of_map_to_rover_x = (rover_origin_x-map_origin_x)/map_width;
float fraction_of_map_to_rover_y = (rover_origin_y-map_origin_y)/map_height;
float x_label_f = (i+1)*max_seen_width/n_ticks-fraction_of_map_to_rover_x*max_seen_width;
float y_label_f = (i+1)*max_seen_height/n_ticks-fraction_of_map_to_rover_y*max_seen_height;
QString x_label = QString::number(x_label_f, 'f', 1) + "m";
QString y_label = QString::number(-y_label_f, 'f', 1) + "m";
int x_labels_offset_x = -(fm.width(x_label))/2;
int x_labels_offset_y = 0;
int y_labels_offset_x = -(fm.width(y_label));
int y_labels_offset_y = fm.height()/3;
painter.drawText(x_axis_ticks[i].x()+x_labels_offset_x, axes_origin.y()+x_labels_offset_y, x_label);
painter.drawText(axes_origin.x()+y_labels_offset_x, y_axis_ticks[i].y()+y_labels_offset_y, y_label);
}
// End draw scale bars
update_mutex.lock();
// scale coordinates
std::vector<QPoint> scaled_target_locations;
for(std::vector< pair<float,float> >::iterator it = target_locations[rover_to_display].begin(); it != target_locations[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
point.setX(map_origin_x+coordinate.first*map_width);
point.setY(map_origin_y+coordinate.second*map_height);
scaled_target_locations.push_back(point);
}
std::vector<QPoint> scaled_collection_points;
for(std::vector< pair<float,float> >::iterator it = collection_points[rover_to_display].begin(); it != collection_points[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
point.setX(map_origin_x+coordinate.first*map_width);
point.setY(map_origin_y+coordinate.second*map_height);
scaled_collection_points.push_back(point);
}
std::vector<QPoint> scaled_gps_rover_points;
for(std::vector< pair<float,float> >::iterator it = gps_rover_path[rover_to_display].begin(); it != gps_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
scaled_gps_rover_points.push_back( QPoint(x,y) );
}
QPainterPath scaled_ekf_rover_path;
for(std::vector< pair<float,float> >::iterator it = ekf_rover_path[rover_to_display].begin(); it != ekf_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
if (it == ekf_rover_path[rover_to_display].begin()) scaled_ekf_rover_path.moveTo(x, y);
scaled_ekf_rover_path.lineTo(x, y);
}
QPainterPath scaled_gps_rover_path;
for(std::vector< pair<float,float> >::iterator it = gps_rover_path[rover_to_display].begin(); it != gps_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
scaled_gps_rover_path.lineTo(x, y);
}
QPainterPath scaled_encoder_rover_path;
for(std::vector< pair<float,float> >::iterator it = encoder_rover_path[rover_to_display].begin(); it != encoder_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
if (it == encoder_rover_path[rover_to_display].begin()) scaled_encoder_rover_path.moveTo(x, y);
scaled_encoder_rover_path.lineTo(x, y);
}
painter.setPen(red);
if (display_gps_data) painter.drawPoints(&scaled_gps_rover_points[0], scaled_gps_rover_points.size());
// if (display_gps_data) painter.drawPath(scaled_gps_rover_path);
painter.setPen(Qt::white);
if (display_ekf_data) painter.drawPath(scaled_ekf_rover_path);
painter.setPen(green);
if (display_encoder_data) painter.drawPath(scaled_encoder_rover_path);
painter.setPen(red);
QPoint* point_array = &scaled_collection_points[0];
painter.drawPoints(point_array, scaled_collection_points.size());
painter.setPen(green);
point_array = &scaled_target_locations[0];
painter.drawPoints(point_array, scaled_target_locations.size());
update_mutex.unlock();
painter.setPen(Qt::white);
}
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();
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
// The RF is formed as this x_axis | y_axis | normal
Eigen::Map<Eigen::Vector3f> x_axis (rf);
Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
Eigen::Map<Eigen::Vector3f> normal (rf + 6);
// Find every point within specified search_radius_
std::vector<int> nn_indices;
std::vector<float> nn_dists;
const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
if (neighb_cnt == 0)
{
for (size_t i = 0; i < desc.size (); ++i)
desc[i] = std::numeric_limits<float>::quiet_NaN ();
memset (rf, 0, sizeof (rf[0]) * 9);
return (false);
}
float minDist = std::numeric_limits<float>::max ();
int minIndex = -1;
for (size_t i = 0; i < nn_indices.size (); i++)
{
if (nn_dists[i] < minDist)
{
minDist = nn_dists[i];
minIndex = nn_indices[i];
}
}
// Get origin point
Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
// Get origin normal
// Use pre-computed normals
normal = normals[minIndex].getNormalVector3fMap ();
// Compute and store the RF direction
x_axis[0] = rnd ();
x_axis[1] = rnd ();
x_axis[2] = rnd ();
if (!pcl::utils::equal (normal[2], 0.0f))
x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
else if (!pcl::utils::equal (normal[1], 0.0f))
x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
else if (!pcl::utils::equal (normal[0], 0.0f))
x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
x_axis.normalize ();
// Check if the computed x axis is orthogonal to the normal
assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
// Store the 3rd frame vector
y_axis = normal.cross (x_axis);
// For each point within radius
for (size_t ne = 0; ne < neighb_cnt; ne++)
{
if (pcl::utils::equal (nn_dists[ne], 0.0f))
continue;
// Get neighbours coordinates
Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
/// ----- Compute current neighbour polar coordinates -----
/// Get distance between the neighbour and the origin
float r = sqrt (nn_dists[ne]);
/// Project point into the tangent plane
Eigen::Vector3f proj;
pcl::geometry::project (neighbour, origin, normal, proj);
proj -= origin;
/// Normalize to compute the dot product
proj.normalize ();
/// Compute the angle between the projection and the x axis in the interval [0,360]
Eigen::Vector3f cross = x_axis.cross (proj);
float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
/// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
Eigen::Vector3f no = neighbour - origin;
no.normalize ();
float theta = normal.dot (no);
theta = pcl::rad2deg (acos (std::min (1.0f, std::max (-1.0f, theta))));
// Bin (j, k, l)
size_t j = 0;
size_t k = 0;
size_t l = 0;
// Compute the Bin(j, k, l) coordinates of current neighbour
for (size_t rad = 1; rad < radius_bins_+1; rad++)
{
if (r <= radii_interval_[rad])
{
j = rad-1;
break;
}
}
for (size_t ang = 1; ang < elevation_bins_+1; ang++)
{
if (theta <= theta_divisions_[ang])
{
k = ang-1;
break;
}
}
for (size_t ang = 1; ang < azimuth_bins_+1; ang++)
{
if (phi <= phi_divisions_[ang])
{
l = ang-1;
break;
}
}
// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
std::vector<float> neighbour_distances;
int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
// point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
if (point_density == 0)
continue;
float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
(k*radius_bins_) +
j];
assert (w >= 0.0);
if (w == std::numeric_limits<float>::infinity ())
PCL_ERROR ("Shape Context Error INF!\n");
if (w != w)
PCL_ERROR ("Shape Context Error IND!\n");
/// Accumulate w into correspondant Bin(j,k,l)
desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
} // end for each neighbour
// 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
memset (rf, 0, sizeof (rf[0]) * 9);
return (true);
}
void Transform::left(float degrees, vec3& eye, vec3& up) {
// YOUR CODE FOR HW2 HERE
vec3 y_axis(0.0, 1.0, 0.0);
eye = eye * rotate(degrees, y_axis);
up = up * rotate(degrees, y_axis);
}
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 Axes::set_range(double y_min, double y_max)
{
(*y_axis()->coords)[0] = QVector2D(0.0, y_min);
(*y_axis()->coords)[1] = QVector2D(0.0, y_max);
}
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");
}
