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