void Camera::updateCachedTransforms() { m_worldToCamera = worldTransform().inverse(); m_view = viewTransform(); m_projection = projectionTransform(); m_viewProjection = viewProjectionTransform(); }
/// Returns the depth of point in the scene. float GraphicsView::depth(const Point3f &point) const { Eigen::Matrix<float, 4, 1> viewPoint; viewPoint[0] = point.x(); viewPoint[1] = point.y(); viewPoint[2] = point.z(); viewPoint[3] = 1; GraphicsTransform transform = projectionTransform() * modelViewTransform(); viewPoint = transform.multiply(viewPoint); viewPoint *= 1.0 / viewPoint[3]; float winZ = (viewPoint[2] + 1) / 2; return winZ; }
/// Unprojects a point from the window to the scene. Point3f GraphicsView::unproject(qreal x, qreal y, qreal z) const { // flip y y = height() - y; // adjust point to normalized window coordinates Eigen::Matrix<float, 4, 1> point; point[0] = 2 * x / width() - 1; point[1] = 2 * y / height() - 1; point[2] = 2 * z - 1; point[3] = 1; // map to object-space coordinates GraphicsTransform transform = projectionTransform() * modelViewTransform(); point = transform.inverseMultiply(point); point *= 1.0 / point[3]; return Point3f(point[0], point[1], point[2]); }
/// Projects a point from the scene to the window. QPointF GraphicsView::project(const Point3f &point) const { Eigen::Matrix<float, 4, 1> vector; vector[0] = point.x(); vector[1] = point.y(); vector[2] = point.z(); vector[3] = 0; GraphicsTransform transform = projectionTransform() * modelViewTransform(); vector = transform.multiply(vector); vector *= 1.0 / vector[3]; float winX = width() * (vector[0] + 1) / 2; float winY = height() * (vector[1] + 1) / 2; float winZ = (vector[2] + 1) / 2; // if winZ is greater than 1.0 the point is not // visible (it is either in front of the near clip // plane or behind the far clip plane). if(winZ > 1.0) return QPointF(); else return QPointF(winX, height() - winY); }
void Camera::render() { synchronized( s_globalMutex ); // - prepare scene rendering // - render objects in n passes // - update statistics // - reset temporaries gd::GraphicsDevice* dev = Context::device(); int renderedPrimitives = dev->renderedPrimitives(); int renderedTriangles = dev->renderedTriangles(); int materialChanges = dev->materialChanges(); // - DEBUG: warn if we have non-square pixels Matrix4x4 proj = projectionTransform(); Vector4 cp( 1,1,10,1 ); Vector4 pp = (proj * cp); pp *= 1.f / pp.w; pp.x = pp.x * (viewportWidth()*.5f); pp.y = pp.y * (viewportHeight()*.5f); if ( Math::abs(pp.x-pp.y) > 1.f ) Debug::println( "Non-square pixels! (fov={0}, w={1}, h={2})", Math::toDegrees(horizontalFov()), viewportWidth(), viewportHeight() ); prepareRender(); // - render objects in n passes // pass 1<<0: solid objects which are affected by shadows // pass 1<<1: (unused) // pass 1<<2: shadow volumes // pass 1<<3: shadow filler polygon // pass 1<<4: solid objects which are not affected by shadows // pass 1<<5: transparent objects which are not affected by shadows // the first pass is front to back... //Debug::println( "{0}({1})", __FILE__, __LINE__ ); {//dev::Profile pr( "Camera.render( pass 1 )" ); int i; for ( i = 0 ; i < (int)s_objs.size() ; ++i ) { Node* obj = s_objs[i]; //Debug::println( "rendering {0}({1})", obj->name(), i ); obj->render( this, 1 ); } } // ...and the rest back to front //Debug::println( "{0}({1})", __FILE__, __LINE__ ); {//dev::Profile pr( "Camera.render( other passes )" ); for ( int pass = 2 ; pass <= LAST_RENDERING_PASS ; pass <<= 1 ) { for ( int i = (int)s_objs.size() ; i-- > 0 ; ) { Node* obj = s_objs[i]; obj->render( this, pass ); } } } // - update statistics m_renderedPrimitives += dev->renderedPrimitives() - renderedPrimitives; m_renderedTriangles += dev->renderedTriangles() - renderedTriangles; m_materialChanges += dev->materialChanges() - materialChanges; // - reset temporaries m_worldToCamera = Matrix4x4(0); dev->setViewport( 0, 0, dev->width(), dev->height() ); //Debug::println( "{0}({1})", __FILE__, __LINE__ ); }
Matrix4x4 Camera::viewProjectionTransform() const { Matrix4x4 view = worldTransform().inverse(); return projectionTransform() * view; }
void Camera::prepareRender() { //Profile pr( "camera.prepareRender" ); // - update transform hierarchy // - find out front and back plane distances // - collect visible objects and lights // - update node visibility // - sort visible objects by ascending distance // - set viewport, view- and projection transformation // - add affecting lights to rendering device // - set fog and ambient if any // - update transform hierarchy Node* root = this->root(); Scene* scene = dynamic_cast<Scene*>( root ); root->validateHierarchy(); updateCachedTransforms(); Vector3 camWorldPos = cachedWorldTransform().translation(); Vector3 camWorldDir = cachedWorldTransform().rotation().getColumn(2); // - collect visible objects and lights s_objs.clear(); s_lights.clear(); for ( Node* obj = root ; obj ; ) { //assert( obj->name().length() > 0 ); obj->m_flags &= ~NODE_RENDEREDINLASTFRAME; if ( obj->enabled() ) { if ( obj->renderable() ) { Light* light = dynamic_cast<Light*>( obj ); if ( light ) { s_lights.add( light ); ++m_renderedLights; } else { obj->m_distanceToCamera = obj->boundSphere() + (obj->m_worldTransform.translation() - camWorldPos).dot( camWorldDir ); if ( obj->updateVisibility(this) ) { obj->m_flags |= NODE_RENDEREDINLASTFRAME; s_objs.add( obj ); ++m_renderedObjects; } } } } ++m_processedObjects; obj = obj->nextInHierarchy( Node::NODE_ENABLED ); } // - sort visible objects by ascending distance std::sort( s_objs.begin(), s_objs.end(), NodeDistanceToCameraLess() ); // - set viewport, view- and projection transformation gd::GraphicsDevice* dev = Context::device(); dev->setViewport( m_x, m_y, viewportWidth(), viewportHeight() ); dev->setViewTransform( m_worldToCamera ); dev->setProjectionTransform( projectionTransform() ); // - add affecting lights to rendering device dev->removeLights(); for ( int i = 0 ; i < (int)s_lights.size() ; ++i ) s_lights[i]->apply(); // - set fog and ambient if any if ( scene ) { setFog( dev, scene ); Colorf amb = Colorf( scene->ambientColor() ); dev->setAmbient( Color(amb) ); } }
int main(int argc, char** argv) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>); if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file { PCL_ERROR("Couldn't read file test_pcd.pcd \n"); return (-1); } pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients(true); // Mandatory seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.01); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { PCL_ERROR("Could not estimate a planar model for the given dataset."); return (-1); } std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(true); //Removes part_of_cloud but retain the original full_cloud //extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest extract.filter(*cloud_plane); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud(cloud_plane); sor.setMeanK(50); sor.setStddevMulThresh(1.0); sor.filter(*cloud_filtered); //cloud.swap(cloud_plane); /* pcl::visualization::CloudViewer viewer("Simple Cloud Viewer"); viewer.showCloud(cloud_plane); while (!viewer.wasStopped()) { } */ Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis; fitted_plane_norm[0] = coefficients->values[0]; fitted_plane_norm[1] = coefficients->values[1]; fitted_plane_norm[2] = coefficients->values[2]; xyaxis_plane_norm[0] = 0.0; xyaxis_plane_norm[1] = 0.0; xyaxis_plane_norm[2] = 1.0; rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm); float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm)); //float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm)); transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis)); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud(*cloud_recentered, *cloud_xyz); /////////////////////////////////////////////////////////////////// Eigen::Vector4f pcaCentroid; pcl::compute3DCentroid(*cloud_xyz, pcaCentroid); Eigen::Matrix3f covariance; computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors); Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors(); std::cout << eigenVectorsPCA.size() << std::endl; if(eigenVectorsPCA.size()!=9) eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity()); projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>()); pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform); // Get the minimum and maximum points of the transformed cloud. pcl::PointXYZ minPoint, maxPoint; pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint); const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap()); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = rgbVis(cloud); // viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud"); // viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2"); viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } /* pcl::PCA< pcl::PointXYZ > pca; pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud; pcl::PointCloud< pcl::PointXYZ > proj; pca.setInputCloud(cloud); pca.project(*cloud, proj); Eigen::Vector4f proj_min; Eigen::Vector4f proj_max; pcl::getMinMax3D(proj, proj_min, proj_max); pcl::PointXYZ min; pcl::PointXYZ max; pca.reconstruct(proj_min, min); pca.reconstruct(proj_max, max); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z); */ return (0); }