点云学习——创建图形

Posted 2021-10-08 Leslie X徐

点云学习——创建图形

创建图形

1.创建线

#include <pcl/visualization/common/shapes.h>

// The following are given (or computed using sample consensus techniques -- see SampleConsensusModelLine)
// Eigen::Vector3f point_on_line, line_direction;
 
pcl::ModelCoefficients line_coeff;
line_coeff.values.resize (6);    // We need 6 values
//起点
line_coeff.values[0] = point_on_line.x ();
line_coeff.values[1] = point_on_line.y ();
line_coeff.values[2] = point_on_line.z ();
//终点
line_coeff.values[3] = line_direction.x ();
line_coeff.values[4] = line_direction.y ();
line_coeff.values[5] = line_direction.z ();
 
viewer->addCube(cube_coeff);

2.创建矩形，使用四元数旋转

Quaternion 四元数

#include <pcl/visualization/common/shapes.h>
#include <QtMath>

void MainWindow::on_actioncreatecube_triggered()
{
    //创建矩形
    pcl::ModelCoefficients cube_coeff;
    //coefficients	the cube coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
    cube_coeff.values.resize(10); 

    //Translation
    //起点
    cube_coeff.values[0] = 0;//Tx
    cube_coeff.values[1] = 0;//Ty
    cube_coeff.values[2] = 0;//Tz

    //Quaternion
    //旋转
    int d[]={0,0,1};//旋转轴方向
    qreal angle = M_PI_4;//旋转角度rad，可使用M_PI、M_PI_4等
    //用四元数表示旋转：某一图形围绕某一向量d旋转了角度angle，其四元数的值如下
    cube_coeff.values[3] = d[0]*qSin(angle/2);//Qx
    cube_coeff.values[4] = d[1]*qSin(angle/2);//Qy
    cube_coeff.values[5] = d[2]*qSin(angle/2);//Qz
    cube_coeff.values[6] = qCos(angle/2);//Qw

    //cube大小
    cube_coeff.values[7] = 1;//width
    cube_coeff.values[8] = 1;//height
    cube_coeff.values[9] = 1;//depth

    viewer->addCube(cube_coeff);
}

图形的移动

Affine 仿射变换

bool pcl::visualization::PCLVisualizer::updateShapePose	(	const std::string & 	id,
const Eigen::Affine3f & 	pose 
)	

移动方块

void MainWindow::on_actionmove_triggered()
{
    Eigen::Affine3f aff({2,2,2}); //输入要移动到的点
    viewer->updateShapePose("cube",aff);
    viewFresh();
}

定时器自动移动

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);   
        
	timer = new QTimer(this);
    timer->stop();
    connect(timer,&QTimer::timeout,this,&MainWindow::move);
}

void MainWindow::move()
{
    static float i=1;
    ++i;
    Eigen::Affine3f aff({i,i,i});
    viewer->updateShapePose("cube",aff);
    viewFresh();
}

void MainWindow::on_actionmove_triggered()
{
    if(timer->isActive())timer->stop();
    else timer->start(1000);
}

路径移动

沿三维直线移动：辗转相除法求方向向量

int MainWindow::gcd(int a, int b)
{
    //辗转相除法，求最大公约数
    if(b==0)return a;
    else return gcd(b,a%b);
}

int MainWindow::gcd3(int a, int b, int c)
{
    //三个数求最大公约数
    return gcd(gcd(a,b),c);
}

void MainWindow::on_actionsetpath_triggered()
{
//p1,p2,p3,cp 都是QVector<float>类型
    p1.resize(3);//路径起点
    p2.resize(3);//路径上某一点
    p3.resize(3);//方向向量
    cp.resize(3);//移动的点
    
    p1={0,0,0};
    p2={12,12,18};
    for (int i=0;i<3 ;++i ) {
        p3[i]=p2[i]-p1[i];
    }
    //计算直线的向量
    int c = gcd3(p3[0],p3[1],p3[2]);
    qDebug()<<c;
    for(auto& i : p3){
        i /= c ;
    }
}

//随时间移动
void MainWindow::move()
{
    static float t=0;
    t+=0.01;

    for (int i=0;i<3 ;++i ) {
        cp[i]=p1[i]+t*p3[i];
    }
    qDebug()<<"x:"<<cp[0]<<" y:"<<cp[1]<<" z:"<<cp[2];

    Eigen::Affine3f aff({cp[0],cp[1],cp[2]});
    viewer->updateShapePose("cube",aff);
    ui->widget->update();

}