예제 #1
0
파일: test_plot.cpp 프로젝트: xalpha/nox
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 );
    }
}
예제 #2
0
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();
}
예제 #3
0
파일: main.cpp 프로젝트: KDE/gluon
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();
}
예제 #4
0
    /**
     * @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);
	}
}
예제 #6
0
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;
}
예제 #7
0
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();
}
예제 #8
0
파일: icp.cpp 프로젝트: zengzhen/ros_zhen
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);
}
예제 #9
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);
}
예제 #10
0
	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);



	}
예제 #11
0
파일: camera.hpp 프로젝트: LCG-UFRJ/tucano
 /**
  * @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);
 }
예제 #12
0
파일: convert.cpp 프로젝트: aginika/mapping
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;
}
예제 #13
0
파일: model.hpp 프로젝트: LCG-UFRJ/tucano
 /**
  * @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);
 }