C++ Affine3f::translate方法代码示例

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

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