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

bool transformCloud(double theta, char axis, double x, double y, double z, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud)
{
	    Eigen::Affine3f transform = Eigen::Affine3f::Identity();

        // Define the translation
        transform.translation() << x, y, z;

        switch( axis ) {
		case 'x':
                 // The same rotation matrix as before; tetha radians arround X axis
		 transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
		 break;
		case 'y':
		 // The same rotation matrix as before; tetha radians arround Y axis
		 transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitY()));
		 break;
		case 'z':
  		 // The same rotation matrix as before; tetha radians arround Z axis
  		 transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
		 break;
	}

  	// Print the transformation
  	//printf ("\nTransformation matrix:\n");
  	//std::cout << transform.matrix() << std::endl;

  	// Executing the transformation
  	
 	 pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform);

	return true;
}

StereoProperties VelStereoMatcher::vecToStereo(const gsl_vector* vec)
{

  float x = gsl_vector_get(vec, 0);
  float y = gsl_vector_get(vec, 1);
  float z = gsl_vector_get(vec, 2);
  float ax = gsl_vector_get(vec, 3);
  float ay = gsl_vector_get(vec, 4);
  float az = gsl_vector_get(vec, 5);
  float r = std::sqrt(ax * ax + ay * ay + az * az);

  float fx = gsl_vector_get(vec, 6);
  float fy = gsl_vector_get(vec, 7);
  float cx = gsl_vector_get(vec, 8);
  float cy = gsl_vector_get(vec, 9);
  float baseline = gsl_vector_get(vec, 10);

  //normalize axis
  ax /= r;
  ay /= r;
  az /= r;

  //create tform
  Eigen::Affine3f tform = Eigen::Affine3f::Identity();
  tform.translation() << x, y, z;
  tform.rotate(Eigen::AngleAxisf(r, Eigen::Vector3f(ax, ay, az)));

  //create stereo
  return StereoProperties(fx,fy,cx,cy,baseline,tform);
}

void fuzzyAffines()
{
    std::vector<Eigen::Matrix4f> trans;
    trans.reserve(count/10);
    for( size_t i=0; i<count/10; i++ )
    {
        Eigen::Vector3f x = Eigen::Vector3f::Random();
        Eigen::Vector3f y = Eigen::Vector3f::Random();

        x.normalize();
        y.normalize();

        Eigen::Vector3f z = x.cross(y);
        z.normalize();

        y = z.cross(x);
        y.normalize();

        Eigen::Affine3f t = Eigen::Affine3f::Identity();
        Eigen::Matrix3f r = Eigen::Matrix3f::Identity();

        r.col(0) = x;
        r.col(1) = y;
        r.col(2) = z;

        t.rotate(r);
        t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );

        trans.push_back( t.matrix() );
    }

    s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
    s_plot.setLineWidth( 3.0 );
    s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}

bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
                     Eigen::Affine3f& transf)
{
  // uz: versor pointing toward the destination
  Eigen::Vector3f uz = target - rayo;
  if (std::abs(uz.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
    return false;
  }
  uz.normalize();
  //std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
  // ux: versor pointing toward the ground
  Eigen::Vector3f ux = down - down.dot(uz) * uz;  
  if (std::abs(ux.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
    return false;
  }
  ux.normalize();
  //std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
  Eigen::Vector3f uy = uz.cross(ux);
  //std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
  Eigen::Matrix3f rot;
  rot << ux.x(), uy.x(), uz.x(),
         ux.y(), uy.y(), uz.y(),
         ux.z(), uy.z(), uz.z();
  transf.setIdentity();
  transf.translate(rayo);
  transf.rotate(rot);
  //std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
  return true;
}

    void rotatePointCloud(pcl::PointCloud<PointT>::Ptr inputCloud, pcl::PointCloud<PointT>::Ptr outputCloud, double theta)
    {
        // Create rotation matrix.
        Eigen::Affine3f transform = Eigen::Affine3f::Identity();
        transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));

        // Transform the cloud and return it
        pcl::transformPointCloud (*inputCloud, *outputCloud, transform);;
    }

PointCloudViewer::PointCloudViewer(QWidget* parent, Qt::WindowFlags f): QVTKWidget(parent, f)
{
    mImpl = new PointCloudViewer::Impl;
    mImpl->Vis.addPointCloud(common::KinectPointCloud::Ptr(new common::KinectPointCloud));
    Eigen::Affine3f trans;
    trans.setIdentity();
    trans.rotate(Eigen::AngleAxisf(3.14159265, Eigen::Vector3f(0, 0, 1)));
    
    mImpl->Vis.addCoordinateSystem(1.0, trans);
    mImpl->Vis.setBackgroundColor(0, 0, 0);
    SetRenderWindow(mImpl->Vis.getRenderWindow().GetPointer());
}

 /* rotate a single pcl::PointXYZ point */
 pcl::PointCloud<pcl::PointXYZ>::Ptr rotateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloudToRotate, 
                                                 float angleToRotateTo){
    
     //create the rotation transformation matrix
     Eigen::Affine3f rotationMatrix = Eigen::Affine3f::Identity();
     rotationMatrix.rotate (Eigen::AngleAxisf (angleToRotateTo, Eigen::Vector3f::UnitZ()));
 
     //Apply rotation
     pcl::PointCloud<pcl::PointXYZ>::Ptr rotatedCloud (new pcl::PointCloud<pcl::PointXYZ> ());
     pcl::transformPointCloud (*cloudToRotate, *rotatedCloud, rotationMatrix);
 
     return rotatedCloud;
 }

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

pcl::PointCloud<pcl::PointXYZ>::Ptr ObjectDetector::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);

    // Create rotation + translation matrix
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 0.0, -y_translation, 0.0;
    transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));

    // Perform transformation on point cloud
    pcl::transformPointCloud(*cloud, *cloud_transformed, transform);

    return cloud_transformed;
}

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

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

Eigen::Affine3f transRotVecToAffine3f(
        const cv::Mat &translationVec, 
        const cv::Mat &rotationVec)
{

    /* Copies the axis angle rotation
     * and the translation to an
     * Affine3f transformation matrix
     */

    /* axis angle roation */
#if 1
    Eigen::Vector3f axis(
            rotationVec.at<float>(0,0),
            rotationVec.at<float>(1,0),
            rotationVec.at<float>(2,0));
    float angle = axis.norm(); // length of the vector 
    axis.normalize();
    Eigen::AngleAxisf rot(angle, axis);
#endif


#if 0
    /* do euler angle rotation */
    Eigen::Affine3f rot;
    rot = Eigen::AngleAxisf(rotationVec.at<float>(0,0), Eigen::Vector3f::UnitX())
      * Eigen::AngleAxisf(rotationVec.at<float>(1,0), Eigen::Vector3f::UnitY())
      * Eigen::AngleAxisf(rotationVec.at<float>(2,0), Eigen::Vector3f::UnitZ());
#endif

    /* compose new pose */
    Eigen::Affine3f pose;
    pose = Eigen::Affine3f (Eigen::Translation3f (
                translationVec.at<float>(0, 0),
                translationVec.at<float>(1, 0),
                translationVec.at<float>(2, 0)));
    pose.rotate(rot);

    return pose;
}

bool normalizeOrientationAndTranslation(std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &points, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals, Eigen::Affine3f &invTransform)
{
    if (points.empty())
        return false;

    // Perform PCA on input to determine a canoncial coordinate frame for the given point cloud.
    Eigen::Matrix3Xf::MapType pointsInMatrix(points.at(0).data(), 3, static_cast<int>(points.size()));
    const Eigen::Vector3f centroid = pointsInMatrix.rowwise().mean();
    pointsInMatrix = pointsInMatrix.colwise() - centroid;

    const Eigen::Matrix3f cov = pointsInMatrix * pointsInMatrix.transpose();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(cov);
    const Eigen::Matrix3f rot = eig.eigenvectors().transpose();
    for (size_t i = 0; i < points.size(); ++i) {
        points[i] = rot * points[i];
        normals[i] = rot * normals[i];
    }

    invTransform = Eigen::Affine3f::Identity();
    invTransform = invTransform.rotate(rot).translate(-centroid); // applied in right to left order.
    invTransform = invTransform.inverse();

    return true;
}

void ObjectDetector::performSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>), cloud_plane_rotated (new pcl::PointCloud<pcl::PointXYZ>);

    ROS_INFO("PointCloud before planar segmentation: %d data points.", cloud->width * cloud->height);

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Fit plane
    seg.setModelType (pcl::SACMODEL_PLANE);
    // Use RANSAC
    seg.setMethodType (pcl::SAC_RANSAC);
    // Set maximum number of iterations
    seg.setMaxIterations (max_it_calibration);
    // Set distance to the model threshold
    seg.setDistanceThreshold (floor_threshold);

    // Segment the largest planar component from the cloud
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);

    // Extract the inliers of the plane
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*cloud_plane);
    ROS_INFO("PointCloud representing the planar component: %d data points.", cloud_plane->width * cloud_plane->height);

    // Create normal vector of the plane
    Eigen::Matrix<float, 1, 3> normal_plane, normal_floor, r_axis;
    normal_plane[0] = coefficients->values[0];
    normal_plane[1] = coefficients->values[1];
    normal_plane[2] = coefficients->values[2];
    ROS_INFO("Plane normal: %f %f %f", normal_plane[0], normal_plane[1], normal_plane[2]);

    // Create normal vector of the floor
    normal_floor[0] = 0.0f;
    normal_floor[1] = 1.0f;
    normal_floor[2] = 0.0f;
    ROS_INFO("Floor normal: %f %f %f", normal_floor[0], normal_floor[1], normal_floor[2]);

    // Determine rotation axis
    r_axis = normal_plane.cross(normal_floor);
    ROS_INFO("Rotation axis: %f %f %f", r_axis[0], r_axis[1], r_axis[2]);

    // Determine rotation angle theta
    theta = acos(normal_plane.dot(normal_floor));
    ROS_INFO("Rotation angle theta: %f", theta);

    // Create rotation matrix
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));

    // Perform rotation on extracted plane
    pcl::transformPointCloud(*cloud_plane, *cloud_plane_rotated,transform);

    // Compute y translation by taking the average y values of the plane points
    int num_of_points = cloud_plane_rotated->width * cloud_plane_rotated->height;
    for (size_t i = 0; i < num_of_points; ++i)
    {
        y_translation += cloud_plane_rotated->points[i].y;
    }
    y_translation = y_translation / num_of_points;
}