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);
}
