void StaticCylinderRenderer::draw( const StaticCylinder& cylinder ) const
{
glPushAttrib( GL_COLOR );
glPushAttrib( GL_LIGHTING );
glPushAttrib( GL_LINE_WIDTH );
glDisable( GL_LIGHTING );
glColor3d( 0.0, 0.0, 0.0 );
glLineWidth( 1.0 );
const Eigen::Matrix<GLfloat,3,1> center{ cylinder.x().cast<GLfloat>() };
const Eigen::Matrix<GLfloat,3,1> translation{ 0.5 * m_L * cylinder.axis().cast<GLfloat>() };
const Eigen::AngleAxis<GLfloat> rotation{ cylinder.R().cast<GLfloat>() };
const GLfloat r{ static_cast<GLfloat>( cylinder.r() ) };
// Draw the top circle
glPushMatrix();
glTranslatef( center.x() + translation.x(), center.y() + translation.y(), center.z() + translation.z() );
glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
glScalef( r, 1.0, r );
glEnableClientState( GL_VERTEX_ARRAY );
glVertexPointer( 3, GL_FLOAT, 0, m_crds.data() );
glDrawArrays( GL_LINE_LOOP, 0, m_crds.size() / 3 );
glDisableClientState( GL_VERTEX_ARRAY );
glPopMatrix();
// Draw the bottom circle
glPushMatrix();
glTranslatef( center.x() - translation.x(), center.y() - translation.y(), center.z() - translation.z() );
glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
glScalef( r, 1.0, r );
glEnableClientState( GL_VERTEX_ARRAY );
glVertexPointer( 3, GL_FLOAT, 0, m_crds.data() );
glDrawArrays( GL_LINE_LOOP, 0, m_crds.size() / 3 );
glDisableClientState( GL_VERTEX_ARRAY );
glPopMatrix();
// Draw the 'supports'
glPushMatrix();
glTranslatef( center.x(), center.y(), center.z() );
glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
glBegin( GL_LINES );
glVertex3d( 0.0, 0.5 * m_L, r );
glVertex3d( 0.0, -0.5 * m_L, r );
glVertex3d( 0.0, 0.5 * m_L, -r );
glVertex3d( 0.0, -0.5 * m_L, -r );
glVertex3d( r, 0.5 * m_L, 0.0 );
glVertex3d( r, -0.5 * m_L, 0.0 );
glVertex3d( -r, 0.5 * m_L, 0.0 );
glVertex3d( -r, -0.5 * m_L, 0.0 );
glEnd();
glPopMatrix();
glPopAttrib();
glPopAttrib();
glPopAttrib();
}
bool getImagePoint( const Eigen::Matrix<Scalar_,3,1>& point, size_t &x, size_t &y ) const
{
if( point.z() <= 0 )
return false;
x = ((point.x() / point.z()) - center_x) / scale_x;
y = ((point.y() / point.z()) - center_y) / scale_y;
// check bounds. x and y are always positive
if( (x >= width) || (y >= height) )
return false;
return true;
}
static void projectWithCam( Eigen::Matrix<double, 2, 1> & sp,
const Eigen::Matrix<double, 3, 1> & p3d,
const Eigen::Matrix<double, 3, 3> & K )
{
sp[ 0 ] = K( 0, 0 ) * p3d.x() / p3d.z() + K( 0, 2 );
sp[ 1 ] = K( 1, 1 ) * p3d.y() / p3d.z() + K( 1, 2 );
}
void OpenGL3DSphereRenderer::drawVertexArray( const Eigen::Matrix<GLfloat,3,1>& primary_color, const Eigen::Matrix<GLfloat,3,1>& secondary_color )
{
const int num_mesh_verts{ static_cast<int>( m_sphere_verts.cols() ) };
glEnableClientState( GL_VERTEX_ARRAY );
glEnableClientState( GL_NORMAL_ARRAY );
assert( m_sphere_verts.cols() == m_sphere_normals.cols() );
glVertexPointer( 3, GL_FLOAT, 0, m_sphere_verts.data() );
glNormalPointer( GL_FLOAT, 0, m_sphere_normals.data() );
// Quad 1
GLfloat mcolorambient[] = { GLfloat(0.3) * primary_color.x(), GLfloat(0.3) * primary_color.y(), GLfloat(0.3) * primary_color.z(), 1.0 };
glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, mcolorambient );
GLfloat mcolordiffuse[] = { primary_color.x(), primary_color.y(), primary_color.z(), (GLfloat) 1.0 };
glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, mcolordiffuse );
glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
// Quad 2
glPushMatrix();
glRotatef( 180.0, 0.0, 0.0, 1.0 );
glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
glPopMatrix();
// Quad 3
GLfloat mcolorambient2[] = { GLfloat(0.3) * secondary_color.x(), GLfloat(0.3) * secondary_color.y(), GLfloat(0.3) * secondary_color.z(), 1.0 };
glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, mcolorambient2 );
GLfloat mcolordiffuse2[] = { secondary_color.x(), secondary_color.y(), secondary_color.z(), 1.0 };
glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, mcolordiffuse2 );
glPushMatrix();
glRotatef( 90.0, 0.0, 0.0, 1.0 );
glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
glPopMatrix();
// Quad 4
glPushMatrix();
glRotatef( 270.0, 0.0, 0.0, 1.0 );
glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
glPopMatrix();
glDisableClientState( GL_NORMAL_ARRAY );
glDisableClientState( GL_VERTEX_ARRAY );
}
void PerspectiveCameraController::positionCamera()
{
{
Eigen::Matrix<GLdouble,4,4> rotation{ Eigen::Matrix<GLdouble,4,4>::Identity() };
rotation.block<1,3>(0,0) = m_cam_y;
rotation.block<1,3>(1,0) = m_cam_x;
rotation.block<1,3>(2,0) = -m_cam_z;
assert( ( rotation.block<3,3>(0,0) * rotation.block<3,3>(0,0).transpose() - Eigen::Matrix<GLdouble,3,3>::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
glMultMatrixd( rotation.data() );
}
{
const Eigen::Matrix<GLdouble,3,1> camera_psn{ m_cam_lookat + m_cam_psn };
glTranslated( -camera_psn.x(), -camera_psn.y(), -camera_psn.z() );
}
}
void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans,
Eigen::Quaternion<double> fq,
const frame_common::CamParams &cp,
bool isFixed, sba::CameraNode &msg)
{
msg.index = index;
msg.transform.translation.x = trans.x();
msg.transform.translation.y = trans.y();
msg.transform.translation.z = trans.z();
msg.transform.rotation.x = fq.x();
msg.transform.rotation.y = fq.y();
msg.transform.rotation.z = fq.z();
msg.transform.rotation.w = fq.w();
msg.fx = cp.fx;
msg.fy = cp.fy;
msg.cx = cp.cx;
msg.cy = cp.cy;
msg.baseline = cp.tx;
msg.fixed = isFixed;
}
template <typename PointT, typename Scalar> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
const Eigen::Matrix<Scalar, 4, 1> ¢roid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
if (indices.empty ())
return (0);
// Initialize to 0
covariance_matrix.setZero ();
size_t point_count;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
point_count = indices.size ();
// For each point in the cloud
for (size_t i = 0; i < point_count; ++i)
{
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = cloud[indices[i]].x - centroid[0];
pt[1] = cloud[indices[i]].y - centroid[1];
pt[2] = cloud[indices[i]].z - centroid[2];
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
point_count = 0;
// For each point in the cloud
for (size_t i = 0; i < indices.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud[indices[i]]))
continue;
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = cloud[indices[i]].x - centroid[0];
pt[1] = cloud[indices[i]].y - centroid[1];
pt[2] = cloud[indices[i]].z - centroid[2];
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
++point_count;
}
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
return (static_cast<unsigned int> (point_count));
}
//+----------------------------------------------------------------------------+
//| main() |
//-----------------------------------------------------------------------------+
int
main(int argc, char** argv)
{
common::PathFileString pfs(argv[0]);
//////////////////////////////////////////////////////////////////////////////
// User command line parser
AnyOption *opt = new AnyOption();
opt->autoUsagePrint(true);
opt->addUsage( "" );
opt->addUsage( "Usage: " );
char buff[256];
sprintf(buff, " Example: %s -r example1_noisy.cfg", pfs.getFileName().c_str());
opt->addUsage( buff );
opt->addUsage( " -h --help Prints this help" );
opt->addUsage( " -r --readfile <filename> Reads the polyhedra description file" );
opt->addUsage( " -t --gltopic <topic name> (opt) ROS Topic to send OpenGL commands " );
opt->addUsage( "" );
opt->setFlag( "help", 'h' );
opt->setOption( "readfile", 'r' );
opt->processCommandArgs( argc, argv );
if( opt->getFlag( "help" ) || opt->getFlag( 'h' ) ) {opt->printUsage(); delete opt; return(1);}
std::string gltopic("OpenGLRosComTopic");
if( opt->getValue( 't' ) != NULL || opt->getValue( "gltopic" ) != NULL ) gltopic = opt->getValue( 't' );
std::cerr << "[OpenGL communication topic is set to : \"" << gltopic << "\"]\n";
std::string readfile;
if( opt->getValue( 'r' ) != NULL || opt->getValue( "readfile" ) != NULL )
{
readfile = opt->getValue( 'r' );
std::cerr << "[ World description is read from \"" << readfile << "\"]\n";
}
else{opt->printUsage(); delete opt; return(-1);}
delete opt;
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Initialize ROS and OpenGLRosCom node (visualization)
std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]"<< std::endl;
ros::init(argc, argv, pfs.getFileName().c_str());
if(ros::isInitialized())
std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]:[OK]\n"<< std::endl;
else{
std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]:[FAILED]\n"<< std::endl;
return(1);
}
COpenGLRosCom glNode;
glNode.CreateNode(gltopic);
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Reading the input file
std::cerr << "Reading the input file..." << std::endl;
ShapeVector shapes;
PoseVector poses;
IDVector ID;
if(ReadPolyhedraConfigFile(readfile, shapes, poses, ID)==false)
{
std::cerr << "["<<pfs.getFileName()<<"]:" << "Error reading file \"" << readfile << "\"\n";
return(-1);
}
std::cerr << "Read " << poses.size() << " poses of " << shapes.size() << " shapes with " << ID.size() << " IDs.\n";
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Visualizing the configuration of objects
std::cerr << "Visualizing the configuration of objects..." << std::endl;
for(unsigned int i=0; i<shapes.size(); i++)
{
for(size_t j=0; j<shapes[i].F.size(); j++)
{
glNode.LineWidth(1.0f);
glNode.AddColor3(0.3f, 0.6f, 0.5f);
glNode.AddBegin("LINE_LOOP");
for(size_t k=0; k<shapes[i].F[j].Idx.size(); k++)
{
int idx = shapes[i].F[j].Idx[k];
Eigen::Matrix<Real,3,1> Vt = poses[i]*shapes[i].V[ idx ];
glNode.AddVertex(Vt.x(), Vt.y(), Vt.z());
}
glNode.AddEnd();
}
}
glNode.SendCMDbuffer();
ros::spinOnce();
common::PressEnterToContinue();
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Running PROMTS with A* search algorithm
std::cerr << "Running PROMTS with Depth Limited search algorithm..." << std::endl;
promts::ProblemDataStruct pds("Depth-Limited Search");
pds.m_glNodePtr = &glNode;
PoseVector refined_poses(shapes.size());
refinePoses_DLSearch(shapes, poses, ID, refined_poses, pds);
//
//////////////////////////////////////////////////////////////////////////////
return(0);
}