void alignedAffines()
{
s_plot.setColor( Eigen::Vector4f(0,0,0,1) );
s_plot.setLineWidth( 1.0 );
for( size_t i=0; i<count; i++ )
{
Eigen::Affine3f t = Eigen::Affine3f::Identity();
t.rotate(Eigen::Matrix3f::Identity());
t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );
s_plot( t.matrix(), nox::plot<float>::Pos | nox::plot<float>::CS );
}
}
void TextComponent::render(const Eigen::Affine3f& parentTrans)
{
Eigen::Affine3f trans = parentTrans * getTransform();
/*Eigen::Vector3f dim(mSize.x(), mSize.y(), 0);
dim = trans * dim - trans.translation();
Renderer::pushClipRect(Eigen::Vector2i((int)trans.translation().x(), (int)trans.translation().y()),
Eigen::Vector2i((int)(dim.x() + 0.5f), (int)(dim.y() + 0.5f)));
*/
if(mTextCache)
{
const Eigen::Vector2f& textSize = mTextCache->metrics.size;
Eigen::Vector3f off(0, (getSize().y() - textSize.y()) / 2.0f, 0);
if(Settings::getInstance()->getBool("DebugText"))
{
// draw the "textbox" area, what we are aligned within
Renderer::setMatrix(trans);
Renderer::drawRect(0.f, 0.f, mSize.x(), mSize.y(), 0xFF000033);
}
trans.translate(off);
trans = roundMatrix(trans);
Renderer::setMatrix(trans);
// draw the text area, where the text actually is going
if(Settings::getInstance()->getBool("DebugText"))
{
switch(mAlignment)
{
case ALIGN_LEFT:
Renderer::drawRect(0.0f, 0.0f, mTextCache->metrics.size.x(), mTextCache->metrics.size.y(), 0x00000033);
break;
case ALIGN_CENTER:
Renderer::drawRect((mSize.x() - mTextCache->metrics.size.x()) / 2.0f, 0.0f, mTextCache->metrics.size.x(), mTextCache->metrics.size.y(), 0x00000033);
break;
case ALIGN_RIGHT:
Renderer::drawRect(mSize.x() - mTextCache->metrics.size.x(), 0.0f, mTextCache->metrics.size.x(), mTextCache->metrics.size.y(), 0x00000033);
break;
}
}
mFont->renderTextCache(mTextCache.get());
}
//Renderer::popClipRect();
}
int main( int argc, char* argv[] )
{
QGuiApplication app( argc, argv );
RenderWindow window;
window.show();
Defaults::initialize();
Eigen::Affine3f mat = Eigen::Affine3f::Identity();
FileMesh* mesh = new FileMesh( GluonCore::DirectoryProvider::instance()->dataDirectory() + "/gluon/examples/graphics/duck.dae" );
GluonCore::ResourceManager::instance()->addResource< Mesh >( "duck.dae", mesh );
mesh->initialize();
Texture* texture = GluonCore::ResourceManager::instance()->createResource< Texture >( "duck.tga" );
texture->load( GluonCore::DirectoryProvider::instance()->dataDirectory() + "/gluon/examples/graphics/duck.tga" );
Material* material = GluonCore::ResourceManager::instance()->createResource< Material>( "duck" );
material->load( GluonCore::DirectoryProvider::instance()->dataDirectory() + "/gluon/examples/graphics/duck.gluonmaterial" );
material->build();
World* world = GluonCore::ResourceManager::instance()->resource< World >( Defaults::World );
Entity* ent = world->createEntity< Entity >();
mat.rotate( Eigen::AngleAxis<float>( -M_PI_4 /* pi/4 */, Eigen::Vector3f(0.f, 1.f, 0.f) ) );
ent->setTransform( mat );
ent->setMesh( mesh );
ent->setMaterialInstance( material->createInstance() );
ent->materialInstance()->setProperty( "texture0", QVariant::fromValue( texture ) );
Camera* cam = world->createEntity< Camera >();
mat = Eigen::Affine3f::Identity();
mat.translate( Eigen::Vector3f(0.f, 75.f, 100.f) );
cam->setTransform( mat );
cam->setVisibleArea( QSizeF( 200.f, 200.f ) );
cam->setNearPlane( 0.f );
cam->setFarPlane( 1000.f );
GluonCore::ResourceManager::instance()->resource< RenderTarget >( Defaults::RenderTarget )->addChild( cam );
//app.exec();
return app.exec();
}
/**
* @brief Renders the trackball representation.
* @todo setTrackballOrthographicMatrix should be set during viewport resize
*/
void render (void)
{
if(drawTrackball)
{
float ratio = (viewport[2] - viewport[0]) / (viewport[3] - viewport[1]);
setTrackballOrthographicMatrix(-ratio, ratio, -1.0, 1.0, 0.1, 100.0);
trackball_shader.bind();
//Using unique viewMatrix for the trackball, considering only the rotation to be visualized.
Eigen::Affine3f trackballViewMatrix = Eigen::Affine3f::Identity();
trackballViewMatrix.translate(defaultTranslation);
trackballViewMatrix.rotate(quaternion);
trackball_shader.setUniform("viewMatrix", trackballViewMatrix);
trackball_shader.setUniform("projectionMatrix", trackballProjectionMatrix);
trackball_shader.setUniform("nearPlane", near_plane);
trackball_shader.setUniform("farPlane", far_plane);
bindBuffers();
//X:
Eigen::Vector4f colorVector(1.0, 0.0, 0.0, 1.0);
trackball_shader.setUniform("modelMatrix", Eigen::Affine3f::Identity()*Eigen::AngleAxis<float>(M_PI/2.0,Eigen::Vector3f(0.0,1.0,0.0)));
trackball_shader.setUniform("in_Color", colorVector);
glDrawArrays(GL_LINE_LOOP, 0, 200);
//Y:
colorVector << 0.0, 1.0, 0.0, 1.0;
trackball_shader.setUniform("modelMatrix", Eigen::Affine3f::Identity()*Eigen::AngleAxis<float>(M_PI/2.0,Eigen::Vector3f(1.0,0.0,0.0)));
trackball_shader.setUniform("in_Color", colorVector);
glDrawArrays(GL_LINE_LOOP, 0, 200);
//Z:
colorVector << 0.0, 0.0, 1.0, 1.0;
trackball_shader.setUniform("modelMatrix", Eigen::Affine3f::Identity());
trackball_shader.setUniform("in_Color", colorVector);
glDrawArrays(GL_LINE_LOOP, 0, 200);
unbindBuffers();
trackball_shader.unbind();
}
}
void TextEditComponent::render(const Eigen::Affine3f& parentTrans)
{
Eigen::Affine3f trans = getTransform() * parentTrans;
renderChildren(trans);
// text + cursor rendering
// offset into our "text area" (padding)
trans.translation() += Eigen::Vector3f(getTextAreaPos().x(), getTextAreaPos().y(), 0);
Eigen::Vector2i clipPos((int)trans.translation().x(), (int)trans.translation().y());
Eigen::Vector3f dimScaled = trans * Eigen::Vector3f(getTextAreaSize().x(), getTextAreaSize().y(), 0); // use "text area" size for clipping
Eigen::Vector2i clipDim((int)dimScaled.x() - trans.translation().x(), (int)dimScaled.y() - trans.translation().y());
Renderer::pushClipRect(clipPos, clipDim);
trans.translate(Eigen::Vector3f(-mScrollOffset.x(), -mScrollOffset.y(), 0));
trans = roundMatrix(trans);
Renderer::setMatrix(trans);
if(mTextCache)
{
mFont->renderTextCache(mTextCache.get());
}
// pop the clip early to allow the cursor to be drawn outside of the "text area"
Renderer::popClipRect();
// draw cursor
if(mEditing)
{
Eigen::Vector2f cursorPos;
if(isMultiline())
{
cursorPos = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor);
}else{
cursorPos = mFont->sizeText(mText.substr(0, mCursor));
cursorPos[1] = 0;
}
float cursorHeight = mFont->getHeight() * 0.8f;
Renderer::drawRect(cursorPos.x(), cursorPos.y() + (mFont->getHeight() - cursorHeight) / 2, 2.0f, cursorHeight, 0x000000FF);
}
}
Eigen::Affine3f interpolateAffine(const Eigen::Affine3f &pose0,
const Eigen::Affine3f &pose1, float blend)
{
/* interpolate translation */
Eigen::Vector3f t0 = pose0.translation();
Eigen::Vector3f t1 = pose1.translation();
Eigen::Vector3f tIP = (t1 - t0)*blend;
/* interpolate rotation */
Eigen::Quaternionf r0(pose0.rotation());
Eigen::Quaternionf r1(pose1.rotation());
Eigen::Quaternionf rIP(r1.slerp(blend, r0));
/* compose resulting pose */
Eigen::Affine3f ipAff = pose0;
ipAff.rotate(rIP);
ipAff.translate(tIP);
return ipAff;
}
void Window::renderLoadingScreen()
{
Eigen::Affine3f trans = Eigen::Affine3f::Identity();
Renderer::setMatrix(trans);
Renderer::drawRect(0, 0, Renderer::getScreenWidth(), Renderer::getScreenHeight(), 0xFFFFFFFF);
ImageComponent splash(this);
splash.setResize(Renderer::getScreenWidth() * 0.6f, 0.0f);
splash.setImage(":/splash.svg");
splash.setPosition((Renderer::getScreenWidth() - splash.getSize().x()) / 2, (Renderer::getScreenHeight() - splash.getSize().y()) / 2 * 0.6f);
splash.render(trans);
auto& font = mDefaultFonts.at(1);
TextCache* cache = font->buildTextCache("LOADING...", 0, 0, 0x656565FF);
trans = trans.translate(Eigen::Vector3f(round((Renderer::getScreenWidth() - cache->metrics.size.x()) / 2.0f),
round(Renderer::getScreenHeight() * 0.835f), 0.0f));
Renderer::setMatrix(trans);
font->renderTextCache(cache);
delete cache;
Renderer::swapBuffers();
}
int
main (int argc, char** argv)
{
// Get input object and scene
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
return (1);
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if(argc<3)
{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
pcl::console::print_error ("Error loading first file!\n");
*cloud_out = *cloud_in;
//transform cloud
Eigen::Affine3f transformation;
transformation.setIdentity();
transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
float roll, pitch, yaw;
roll = 0.02; pitch = 1.2; yaw = 0;
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
transformation.rotate(q);
pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
}else{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
{
pcl::console::print_error ("Error loading files!\n");
return (1);
}
}
// Fill in the CloudIn data
// cloud_in->width = 100;
// cloud_in->height = 1;
// cloud_in->is_dense = false;
// cloud_in->points.resize (cloud_in->width * cloud_in->height);
// for (size_t i = 0; i < cloud_in->points.size (); ++i)
// {
// cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
// }
std::cout << "size:" << cloud_out->points.size() << std::endl;
{
pcl::ScopeTime("icp proces");
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZRGBA> Final;
icp.setMaximumIterations(1000000);
icp.setRANSACOutlierRejectionThreshold(0.01);
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
//translation, rotation
Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
std::cout << "rotation: " << euler.transpose() << std::endl;
std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
}
return (0);
}
TEST (PCL, GSHOTWithRTransNoised)
{
PointCloud<PointNormal>::Ptr cloud_nr (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_rot (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_trans (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_rot_trans (new PointCloud<PointNormal> ());
PointCloud<PointXYZ>::Ptr cloud_noise (new PointCloud<PointXYZ> ());
Eigen::Affine3f rot = Eigen::Affine3f::Identity ();
float rot_x = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
float rot_y = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
float rot_z = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
rot.prerotate (Eigen::AngleAxisf (rot_x * M_PI, Eigen::Vector3f::UnitX ()));
rot.prerotate (Eigen::AngleAxisf (rot_y * M_PI, Eigen::Vector3f::UnitY ()));
rot.prerotate (Eigen::AngleAxisf (rot_z * M_PI, Eigen::Vector3f::UnitZ ()));
//std::cout << "rot = (" << (rot_x * M_PI) << ", " << (rot_y * M_PI) << ", " << (rot_z * M_PI) << ")" << std::endl;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
float HI = 5.0f;
float LO = -HI;
float trans_x = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
float trans_y = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
float trans_z = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
//std::cout << "trans = (" << trans_x << ", " << trans_y << ", " << trans_z << ")" << std::endl;
trans.translate (Eigen::Vector3f (trans_x, trans_y, trans_z));
// Estimate normals first
float mr = 0.002;
NormalEstimation<PointXYZ, pcl::Normal> n;
PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
n.setViewPoint (0.0, 0.0, 1.0);
n.setInputCloud (cloud.makeShared ());
n.setRadiusSearch (20 * mr);
n.compute (*normals1);
pcl::concatenateFields<PointXYZ, Normal, PointNormal> (cloud, *normals1, *cloud_nr);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_rot, rot);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_trans, trans);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_rot, *cloud_rot_trans, trans);
add_gaussian_noise (cloud.makeShared (), cloud_noise, 0.005);
PointCloud<Normal>::Ptr normals_noise (new PointCloud<Normal> ());
n.setInputCloud (cloud_noise);
n.compute (*normals_noise);
PointCloud<Normal>::Ptr normals2 (new PointCloud<Normal> ());
n.setInputCloud (cloud2.makeShared ());
n.compute (*normals2);
PointCloud<Normal>::Ptr normals3 (new PointCloud<Normal> ());
n.setInputCloud (cloud3.makeShared ());
n.compute (*normals3);
// Objects
// Descriptors for ground truth (using SHOT)
PointCloud<SHOT352>::Ptr desc01 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc02 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc03 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc04 (new PointCloud<SHOT352> ());
// Descriptors for test GSHOT
PointCloud<SHOT352>::Ptr desc1 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc2 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc3 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc4 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc5 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc6 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc7 (new PointCloud<SHOT352> ());
// SHOT352 (global)
GSHOTEstimation<PointNormal, PointNormal, SHOT352> gshot1;
gshot1.setInputNormals (cloud_nr);
gshot1.setInputCloud (cloud_nr);
gshot1.compute (*desc1);
// Eigen::Vector4f center_desc1 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_rot);
gshot1.setInputCloud (cloud_rot);
gshot1.compute (*desc2);
// Eigen::Vector4f center_desc2 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_trans);
gshot1.setInputCloud (cloud_trans);
gshot1.compute (*desc3);
// Eigen::Vector4f center_desc3 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_rot_trans);
gshot1.setInputCloud (cloud_rot_trans);
gshot1.compute (*desc4);
// Eigen::Vector4f center_desc4 = gshot.getCentralPoint ();
GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot2;
gshot2.setInputNormals (normals1);
gshot2.setInputCloud (cloud_noise);
gshot2.compute (*desc5);
gshot2.setInputNormals (normals2);
gshot2.setInputCloud (cloud2.makeShared ());
gshot2.compute (*desc6);
gshot2.setInputNormals (normals3);
gshot2.setInputCloud (cloud3.makeShared ());
gshot2.compute (*desc7);
// Eigen::Vector3f distance_desc = (center_desc3.head<3> () - center_desc1.head<3> ());
// std::cout << "dist of desc0 and desc3 -> (" << distance_desc[0] << ", " << distance_desc[1] << ", " << distance_desc[2] << ")\n";
// SHOT352 (local)
GSHOTEstimation<PointNormal, PointNormal, SHOT352> shot;
shot.setInputNormals (cloud_nr);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_nr);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc01);
shot.setInputNormals (cloud_rot);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_rot);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc02);
shot.setInputNormals (cloud_trans);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_trans);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc03);
shot.setInputNormals (cloud_rot_trans);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_rot_trans);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc04);
// CHECK GSHOT
checkDesc(*desc01, *desc1);
checkDesc(*desc02, *desc2);
checkDesc(*desc03, *desc3);
checkDesc(*desc04, *desc4);
std::vector<float> d0, d1, d2, d3, d4, d5, d6;
for(int i = 0; i < 352; ++i)
{
d0.push_back(desc1->points[0].descriptor[i]);
d1.push_back(desc2->points[0].descriptor[i]);
d2.push_back(desc3->points[0].descriptor[i]);
d3.push_back(desc4->points[0].descriptor[i]);
d4.push_back(desc5->points[0].descriptor[i]);
d5.push_back(desc6->points[0].descriptor[i]);
d6.push_back(desc7->points[0].descriptor[i]);
}
float dist_0 = pcl::selectNorm< std::vector<float> > (d0, d0, 352, pcl::HIK) ;
float dist_1 = pcl::selectNorm< std::vector<float> > (d0, d1, 352, pcl::HIK) ;
float dist_2 = pcl::selectNorm< std::vector<float> > (d0, d2, 352, pcl::HIK) ;
float dist_3 = pcl::selectNorm< std::vector<float> > (d0, d3, 352, pcl::HIK) ;
float dist_4 = pcl::selectNorm< std::vector<float> > (d0, d4, 352, pcl::HIK) ;
float dist_5 = pcl::selectNorm< std::vector<float> > (d0, d5, 352, pcl::HIK) ;
float dist_6 = pcl::selectNorm< std::vector<float> > (d0, d6, 352, pcl::HIK) ;
std::cout << ">> Itself[HIK]: " << dist_0 << std::endl
<< ">> Rotation[HIK]: " << dist_1 << std::endl
<< ">> Translate[HIK]: " << dist_2 << std::endl
<< ">> Rot+Trans[HIK] " << dist_3 << std::endl
<< ">> GaussNoise[HIK]: " << dist_4 << std::endl
<< ">> bun03[HIK]: " << dist_5 << std::endl
<< ">> milk[HIK]: " << dist_6 << std::endl;
float high_barrier = dist_0 * 0.999f;
float noise_barrier = dist_0 * 0.75f;
float cut_barrier = dist_0 * 0.20f;
float low_barrier = dist_0 * 0.02f;
EXPECT_GT (dist_1, high_barrier);
EXPECT_GT (dist_2, high_barrier);
//EXPECT_GT (dist_3, high_barrier);
EXPECT_GT (dist_4, noise_barrier);
EXPECT_GT (dist_5, cut_barrier);
EXPECT_LT (dist_6, low_barrier);
}
void cloud_cb(
const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) {
iter++;
if(iter != skip) return;
iter = 0;
pcl::PointCloud<pcl::PointXYZRGB> cloud_transformed,
cloud_aligned, cloud_filtered;
Eigen::Affine3f view_transform;
view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
Eigen::AngleAxis<float> rot(tilt * M_PI / 180,
Eigen::Vector3f(0, 1, 0));
view_transform.prerotate(rot);
pcl::transformPointCloud(*cloud, cloud_transformed, view_transform);
pcl::ModelCoefficients::Ptr coefficients(
new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);
seg.setProbability(0.99);
seg.setInputCloud(cloud_transformed.makeShared());
seg.segment(*inliers, *coefficients);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud_transformed.makeShared());
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(cloud_transformed);
std::cout << "Z vector: " << coefficients->values[0] << " "
<< coefficients->values[1] << " " << coefficients->values[2]
<< " " << coefficients->values[3] << std::endl;
Eigen::Vector3f z_current(coefficients->values[0],
coefficients->values[1], coefficients->values[2]);
Eigen::Vector3f y(0, 1, 0);
Eigen::Affine3f rotation;
rotation = pcl::getTransFromUnitVectorsZY(z_current, y);
rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));
pcl::transformPointCloud(cloud_transformed, cloud_aligned, rotation);
Eigen::Affine3f res = (rotation * view_transform);
cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
cloud_aligned.sensor_orientation_ = res.rotate(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
seg.setInputCloud(cloud_aligned.makeShared());
seg.segment(*inliers, *coefficients);
std::cout << "Z vector: " << coefficients->values[0] << " "
<< coefficients->values[1] << " " << coefficients->values[2]
<< " " << coefficients->values[3] << std::endl;
pub.publish(cloud_aligned);
}
/**
* @brief Translates the view matrix by a given vector.
* @param translation Translation to apply to view matrix.
*/
void translate (const Eigen::Vector3f& translation)
{
view_matrix.translate(translation);
}
int main(int argc, char** argv)
{
if (argc < 5)
{
PCL17_INFO ("Usage %s -input_file /in_file -output_file /out_file [options]\n", argv[0]);
PCL17_INFO (" * where options are:\n"
" -tilt <X> : tilt. Default : 30\n"
"");
return -1;
}
int tilt = 30;
std::string input_file;
std::string output_file;
pcl17::console::parse_argument(argc, argv, "-input_file", input_file);
pcl17::console::parse_argument(argc, argv, "-output_file", output_file);
pcl17::console::parse_argument(argc, argv, "-tilt", tilt);
pcl17::PointCloud<pcl17::PointXYZRGB> cloud, cloud_transformed, cloud_aligned, cloud_filtered;
pcl17::io::loadPCDFile(input_file, cloud);
Eigen::Affine3f view_transform;
view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
Eigen::AngleAxis<float> rot(tilt * M_PI / 180, Eigen::Vector3f(0, 1, 0));
view_transform.prerotate(rot);
pcl17::transformPointCloud(cloud, cloud_transformed, view_transform);
pcl17::ModelCoefficients::Ptr coefficients(new pcl17::ModelCoefficients);
pcl17::PointIndices::Ptr inliers(new pcl17::PointIndices);
pcl17::SACSegmentation<pcl17::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl17::SACMODEL_PLANE);
seg.setMethodType(pcl17::SAC_RANSAC);
seg.setDistanceThreshold(0.05);
seg.setProbability(0.99);
seg.setInputCloud(cloud_transformed.makeShared());
seg.segment(*inliers, *coefficients);
pcl17::ExtractIndices<pcl17::PointXYZRGB> extract;
extract.setInputCloud(cloud_transformed.makeShared());
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(cloud_transformed);
std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " "
<< coefficients->values[2] << " " << coefficients->values[3] << std::endl;
Eigen::Vector3f z_current(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
Eigen::Vector3f y(0, 1, 0);
Eigen::Affine3f rotation;
rotation = pcl17::getTransFromUnitVectorsZY(z_current, y);
rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));
pcl17::transformPointCloud(cloud_transformed, cloud_aligned, rotation);
Eigen::Affine3f res = (rotation * view_transform);
cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
cloud_aligned.sensor_orientation_ = res.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
seg.setInputCloud(cloud_aligned.makeShared());
seg.segment(*inliers, *coefficients);
std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " "
<< coefficients->values[2] << " " << coefficients->values[3] << std::endl;
pcl17::io::savePCDFile(output_file, cloud_aligned);
return 0;
}
/**
* @brief Normalize model matrix to center and scale model.
* The model matrix will include a translation to place model's centroid
* at the origin, and scale the model to fit inside a unit sphere.
*/
void normalizeModelMatrix (void)
{
model_matrix.scale(scale);
model_matrix.translate(-centroid);
}