void Mesh::applyTransform(const Pose3D& pose) { foreach_idx(i, vertices) vertices[i] = pose.cameraTransform(vertices[i]); ntk::Pose3D normal_pose; normal_pose.applyTransformBefore(cv::Vec3f(0.f,0.f,0.f), pose.cvEulerRotation()); foreach_idx(i, normals) normals[i] = normal_pose.cameraTransform(normals[i]); }
void MeshRenderer :: setPose(const Pose3D& pose, float* arg_near_plane, float* arg_far_plane) { VertexBufferObject& vbo = m_vertex_buffer_object; pose.cvCameraTransform().copyTo(vbo.model_view_matrix); // Transpose the matrix for OpenGL column-major. vbo.model_view_matrix = vbo.model_view_matrix.t(); if (!(arg_near_plane && arg_far_plane)) { estimateOptimalPlanes(pose, &m_last_near_plane, &m_last_far_plane); } else { m_last_near_plane = *arg_near_plane; m_last_far_plane = *arg_far_plane; } m_pbuffer->makeCurrent(); glMatrixMode (GL_MODELVIEW); glLoadIdentity (); cv::Vec3f euler_angles = pose.cvEulerRotation(); glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]); glRotatef(euler_angles[2]*180.0/M_PI, 0, 0, 1); glRotatef(euler_angles[1]*180.0/M_PI, 0, 1, 0); glRotatef(euler_angles[0]*180.0/M_PI, 1, 0, 0); glMatrixMode (GL_PROJECTION); glLoadIdentity (); double dx = pose.imageCenterX() - (m_pbuffer->width() / 2.0); double dy = pose.imageCenterY() - (m_pbuffer->height() / 2.0); glViewport(dx, -dy, m_pbuffer->width(), m_pbuffer->height()); if (pose.isOrthographic()) { ntk_dbg_print(pose.focalX()/2, 0); ntk_dbg_print(pose.focalY()/2, 0); glOrtho(-pose.focalX()/2, pose.focalX()/2, -pose.focalY()/2, pose.focalY()/2, m_last_near_plane, m_last_far_plane); } else { double fov = (180.0/M_PI) * 2.0*atan(m_pbuffer->height()/(2.0*pose.focalY())); // double fov2 = (180.0/M_PI) * 2.0*atan(image.cols/(2.0*pose.focalX())); // ntk_dbg_print(fov2, 2); // gluPerspective(fov2, double(image.rows)/image.cols, near_plane, far_plane); gluPerspective(fov, double(m_pbuffer->width())/m_pbuffer->height(), m_last_near_plane, m_last_far_plane); } glMatrixMode (GL_MODELVIEW); }
void CalibrationMeshViewer::onCameraPositionUpdate(const cv::Vec3f &translation, const cv::Vec3f &rotation) { if (!m_calibration_mode) { MeshViewer::onCameraPositionUpdate(translation, rotation); return; } GLdouble m[16]; GLdouble deltam[16]; const float rotation_scale = 0.2; const float translation_scale = 0.2; // Get the delta transformation is visualization frame. makeCurrent(); glMatrixMode(GL_MODELVIEW); glGetDoublev(GL_MODELVIEW_MATRIX, m); glLoadIdentity(); glTranslatef(translation_scale*translation[0],translation_scale*translation[1],translation_scale*translation[2]); glTranslatef(m_display_center.x,m_display_center.y,m_display_center.z); glRotatef(rotation_scale*rotation[0], 0,1,0); glRotatef(rotation_scale*rotation[1], 1,0,0); glTranslatef(-m_display_center.x,-m_display_center.y,-m_display_center.z); glGetDoublev(GL_MODELVIEW_MATRIX, deltam); glLoadMatrixd(m); cv::Vec3f t,r; window->getCalibration(t, r); Pose3D p_old; p_old.applyTransformBefore(t, r); cv::Mat1d H_old = p_old.cvCameraTransformd(); cv::Mat1d H(4,4,(double*)deltam); H = H.t(); // delta rotation AFTER model view matrix cv::Mat1d M(4,4,(double*)m); M = M.t(); // model view matrix cv::Mat1d Hp = (M.inv() * H * M * H_old.inv()).inv(); // delta rotation BEFORE model view matrix Pose3D p; p.setCameraTransform(Hp); window->updateFromCalibration(p.cvTranslation(), p.cvEulerRotation()); window->updateToCalibration(); }
void MeshRenderer :: computeProjectionMatrix(cv::Mat4b& image, const Pose3D& pose) { double near_plane, far_plane; estimateOptimalPlanes(pose, &near_plane, &far_plane); ntk_dbg_print(near_plane, 2); ntk_dbg_print(far_plane, 2); m_last_near_plane = near_plane; m_last_far_plane = far_plane; m_pbuffer->makeCurrent(); glMatrixMode (GL_MODELVIEW); glLoadIdentity (); cv::Vec3f euler_angles = pose.cvEulerRotation(); glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]); glRotatef(euler_angles[2]*180.0/M_PI, 0, 0, 1); glRotatef(euler_angles[1]*180.0/M_PI, 0, 1, 0); glRotatef(euler_angles[0]*180.0/M_PI, 1, 0, 0); glMatrixMode (GL_PROJECTION); glLoadIdentity (); double dx = pose.imageCenterX() - (image.cols / 2.0); double dy = pose.imageCenterY() - (image.rows / 2.0); glViewport(dx, -dy, image.cols, image.rows); //glViewport(0, 0, image.cols, image.rows); if (pose.isOrthographic()) { gluOrtho2D(-pose.focalX()/2, pose.focalX()/2, -pose.focalY()/2, pose.focalY()/2); } else { double fov = (180.0/M_PI) * 2.0*atan(image.rows/(2.0*pose.focalY())); // double fov2 = (180.0/M_PI) * 2.0*atan(image.cols/(2.0*pose.focalX())); // ntk_dbg_print(fov2, 2); // gluPerspective(fov2, double(image.rows)/image.cols, near_plane, far_plane); gluPerspective(fov, double(image.cols)/image.rows, near_plane, far_plane); } glMatrixMode (GL_MODELVIEW); }
bool SurfelsRGBDModeler :: addNewView(const RGBDImage& image_, Pose3D& depth_pose) { ntk::TimeCount tc("SurfelsRGBDModeler::addNewView", 1); const float max_camera_normal_angle = ntk::deg_to_rad(90); RGBDImage image; image_.copyTo(image); if (!image_.normal().data) { OpenniRGBDProcessor processor; processor.computeNormalsPCL(image); } Pose3D rgb_pose = depth_pose; rgb_pose.toRightCamera(image.calibration()->rgb_intrinsics, image.calibration()->R, image.calibration()->T); Pose3D world_to_camera_normal_pose; world_to_camera_normal_pose.applyTransformBefore(cv::Vec3f(0,0,0), depth_pose.cvEulerRotation()); Pose3D camera_to_world_normal_pose = world_to_camera_normal_pose; camera_to_world_normal_pose.invert(); const Mat1f& depth_im = image.depth(); Mat1b covered_pixels (depth_im.size()); covered_pixels = 0; std::list<Surfel> surfels_to_reinsert; // Surfel updating. for (SurfelMap::iterator next_it = m_surfels.begin(); next_it != m_surfels.end(); ) { SurfelMap::iterator surfel_it = next_it; ++next_it; Surfel& surfel = surfel_it->second; if (!surfel.enabled()) continue; Point3f surfel_2d = depth_pose.projectToImage(surfel.location); bool surfel_deleted = false; int r = ntk::math::rnd(surfel_2d.y); int c = ntk::math::rnd(surfel_2d.x); int d = ntk::math::rnd(surfel_2d.z); if (!is_yx_in_range(depth_im, r, c) || !image.depthMask()(r, c) || !image.isValidNormal(r,c)) continue; const float update_max_dist = getCompatibilityDistance(depth_im(r,c)); Vec3f camera_normal = image.normal()(r, c); normalize(camera_normal); Vec3f world_normal = camera_to_world_normal_pose.cameraTransform(camera_normal); normalize(world_normal); Vec3f eyev = camera_eye_vector(depth_pose, r, c); double camera_angle = acos(camera_normal.dot(-eyev)); if (camera_angle > max_camera_normal_angle) continue; float normal_angle = acos(world_normal.dot(surfel.normal)); // Surfels have different normals, maybe two different faces of the same object. if (normal_angle > (m_update_max_normal_angle*M_PI/180.0)) { // Removal check. If a surfel has a different normal and is closer to the camera // than the new scan, remove it. if ((-surfel_2d.z) < depth_im(r,c) && surfel.n_views < 3) { m_surfels.erase(surfel_it); surfel_deleted = true; } continue; } // If existing surfel is far from new depth value: // - If existing one had a worst point of view, and was seen only once, remove it. // - Otherwise do not include the new one. if (std::abs(surfel_2d.z - depth_im(r,c)) > update_max_dist) { if (surfel.min_camera_angle > camera_angle && surfel.n_views < 3) { m_surfels.erase(surfel_it); surfel_deleted = true; } else covered_pixels(r,c) = 1; continue; } // Compatible surfel found. const float depth = depth_im(r,c) + m_global_depth_offset; Point3f p3d = depth_pose.unprojectFromImage(Point2f(c,r), depth); cv::Vec3b rgb_color = bgr_to_rgb(image.mappedRgb()(r, c)); Surfel image_surfel; image_surfel.location = p3d; image_surfel.normal = world_normal; image_surfel.color = rgb_color; image_surfel.min_camera_angle = camera_angle; image_surfel.n_views = 1; image_surfel.radius = computeSurfelRadius(depth, camera_normal[2], depth_pose.meanFocal()); mergeToLeftSurfel(surfel, image_surfel); covered_pixels(r,c) = 1; // needs to change the cell? Cell new_cell = worldToCell(surfel.location); if (new_cell != surfel_it->first) { surfels_to_reinsert.push_back(surfel); m_surfels.erase(surfel_it); } } foreach_const_it(it, surfels_to_reinsert, std::list<Surfel>) { Cell new_cell = worldToCell(it->location); m_surfels.insert(std::make_pair(new_cell, *it)); }
void MeshViewer :: addMeshToDisplayList(const ntk::Mesh& mesh, const Pose3D& pose, MeshViewerMode mode) { int new_list_index = glGenLists(1); glNewList(new_list_index, GL_COMPILE); if (mesh.texture.data) { // Last texture id was just created GLuint texture = m_upcoming_textures[m_upcoming_textures.size()-1]; glEnable(GL_TEXTURE_2D); glBindTexture( GL_TEXTURE_2D, texture ); } else { glDisable(GL_TEXTURE_2D); } glMatrixMode(GL_MODELVIEW); glPushMatrix(); glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]); Vec3f euler_angles = pose.cvEulerRotation(); glRotatef(rad_to_deg(euler_angles[0]), 1, 0, 0); glRotatef(rad_to_deg(euler_angles[1]), 0, 1, 0); glRotatef(rad_to_deg(euler_angles[2]), 0, 0, 1); if (mode & WIREFRAME) { glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); } else { glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); } int64 point_start_time = ntk::Time::getMillisecondCounter(); if (mesh.faces.size() == 0) { glBegin(GL_POINTS); for (int i = 0; i < mesh.vertices.size(); ++i) { const Point3f& v = mesh.vertices[i]; // if (i % 1000 == 0) // ntk_dbg_print(v, 1); if (mesh.hasColors()) glColor3f(mesh.colors[i][0]/255.0, mesh.colors[i][1]/255.0, mesh.colors[i][2]/255.0); glVertex3f(v.x, v.y, v.z); } glEnd(); } int64 point_end_time = ntk::Time::getMillisecondCounter(); ntk_dbg_print(point_end_time-point_start_time, 1); { glBegin(GL_TRIANGLES); for (int i = 0; i < mesh.faces.size(); ++i) { int i1 = mesh.faces[i].indices[0]; int i2 = mesh.faces[i].indices[1]; int i3 = mesh.faces[i].indices[2]; const Point3f& v1 = mesh.vertices[i1]; const Point3f& v2 = mesh.vertices[i2]; const Point3f& v3 = mesh.vertices[i3]; Vec3f nm = (Vec3f(v2-v1).cross(v3-v2)); normalize(nm); if (!mesh.hasColors()) glColor3f(1.0f, 0.0f, 0.0f); if (mesh.hasColors()) glColor3f(mesh.colors[i1][0]/255.0, mesh.colors[i1][1]/255.0, mesh.colors[i1][2]/255.0); if (mesh.hasTexcoords()) glTexCoord2f(mesh.texcoords[i1].x, mesh.texcoords[i1].y); glVertex3f(v1.x, v1.y, v1.z); glNormal3f(nm[0], nm[1], nm[2]); if (mesh.hasColors()) glColor3f(mesh.colors[i2][0]/255.0, mesh.colors[i2][1]/255.0, mesh.colors[i2][2]/255.0); if (mesh.hasTexcoords()) glTexCoord2f(mesh.texcoords[i2].x, mesh.texcoords[i2].y); glVertex3f(v2.x, v2.y, v2.z); glNormal3f(nm[0], nm[1], nm[2]); if (mesh.hasColors()) glColor3f(mesh.colors[i3][0]/255.0, mesh.colors[i3][1]/255.0, mesh.colors[i3][2]/255.0); if (mesh.hasTexcoords()) glTexCoord2f(mesh.texcoords[i3].x, mesh.texcoords[i3].y); glVertex3f(v3.x, v3.y, v3.z); glNormal3f(nm[0], nm[1], nm[2]); } glEnd(); } glMatrixMode(GL_MODELVIEW); glPopMatrix(); glEndList(); m_upcoming_display_lists.push_back(new_list_index); }