三维扫描仪[10]——如何设计一台云台式扫描仪（代码详解）

drawPointCloud方法

void drawPointCloud(int steps)
{
int index;
PVector realWorldPoint;
stroke(255);

for(int y = 0; y < kinect.depthHeight(); y += steps) {
for(int x = 0; x < kinect.depthWidth(); x += steps) {
index = x + y * kinect.depthWidth();
realWorldPoint = kinect.depthMapRealWorld()[index];
stroke(150);
point(realWorldPoint.x, realWorldPoint.y, realWorldPoint.z);
}
}
}

绘制点云。没有色彩。根据传入的参数调整是否需要跳过部分点。

index = x + y * kinect.depthWidth();

Kinect有彩色摄像头和深度摄像头，这句话就是找到深度摄像头的第一个点。

当然，你要把彩色摄像头和深度摄像头放在一起显示，你会发现他们高、宽都是一样的。



关于stroke请参考官方文档。

关于PVector请参考官方文档。请一定要参考！

drawObjects方法

void drawObjects()
{
pushStyle();
strokeWeight(1);

for(int i = 1; i < objectPoints.size(); i++) {
stroke(objectColors.get(i).x, objectColors.get(i).y, objectColors.get(i).z);
point(objectPoints.get(i).x, objectPoints.get(i).y, objectPoints.get(i).z + axis.z);
}
for(int i = 1; i < scanPoints.size(); i++) {
stroke(scanColors.get(i).x, scanColors.get(i).y, scanColors.get(i).z);
point(scanPoints.get(i).x, scanPoints.get(i).y, scanPoints.get(i).z + axis.z);
}
popStyle();
}

首先我们要知道objectPoints的大小，所以我们需要objectPoints.size()这个属性。

objectPoints是个结构体，当前objectPoints用objectColors.get(i)来表示，当前的一个x y z当然就是再取一个——objectColors.get(i).x。

我们冷静一下：

首先知道一共有多少像素，所以有objectPoints.size()

我们开始遍历，遍历到你了，你就用objectColors.get(i)来表示

那么你又是一个结构体，所以你的一个属性当然就是objectColors.get(i).x, objectColors.get(i).y, objectColors.get(i).z

scanPoints同理。

还记得axis吗？参考一下我的上一篇博客

有这一句对吧：PVector axis = new PVector(0, baseHeight, 1200);

好，我这里详细讲一下两个坐标系的转换。

最终，这个方法首先显示出objectPoints数组列表中的点，也就是拼在一起的点。然后显示出存储在scanPoints数组列表中的点，也就是正在扫描的点，实时的点。所有点均以真实颜色显示。

坐标系

看啊！反面教材啊！这就是坐标系没搞好的情况！

这个被扫描的是什么呢？是个球，你知道是个球就行了……

首先，Kinect是放在那里不动的对吧，那么动起来的是什么?

是转盘是吧！

转盘是圆的是吧！圆的就有圆心是吧！

重点！

！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！

！我们需要以转盘的圆心为中心，旋转并叠加我们的点云！否则就会出现上面这种情况！

！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！

动起来的转盘是不是一个坐标系，静止的Kinect是不是又是一个坐标系！

我们是不是要赋予Kinect一个圆心的概念！要不然Kinect怎么可能知道它的点云应该围着哪转是吧

我们需要自己设置一个向量，这个向量的位置依你的机械结构而定，最终

！！！！！！！！！！！！！！！

！这个向量就应处在转盘圆心处！

！！！！！！！！！！！！！！！

PVector axis = new PVector(0, baseHeight, 1200);

Kinect和转盘并没有X轴上的偏移，所以X=0

Kinect和转盘的高度有所不同，所以Y=baseHeight

Kinect和转盘的距离较远，所以Z=1200

我转盘的圆心，距离Kinect有1200那么远，所以我设置为1200。你可以调整这个参数，看看效果。我文字描述得再清楚，也比不上看上一眼，那瞬间的领悟。

drawBoundingBox 方法

void drawBoundingBox()
{
stroke(255, 0, 0);
line(axis.x, axis.y, axis.z, axis.x, axis.y+100, axis.z);
noFill();
pushMatrix();
translate(axis.x, axis.x
4000
+ baseHeight + (modelHeight / 2), axis.z);
box(modelWidth, modelHeight, modelWidth);
popMatrix();
}

translate函数可以参考我之前的博客。

绘制出转盘圆心，是一条竖线。

绘制出扫描范围，是个盒子。

scan方法

void scan()
{
for(PVector v : scanPoints) {
objectPoints.add(v.get());
int index = scanPoints.indexOf(v);
objectColors.add(scanColors.get(index).get());
}
if(currentShot < shotNumber.length-1) {
currentShot ++;
println("scan angle = " + Angle);
}
else {
scanning = false;
}
arrived = false;
}

把当前扫描到的数据（数组）scanPoints塞入数组objectPoints。

重要方法！updateObject方法及vecRotY方法

updateObject()函数计算的是扫描点的世界坐标所在的位置。先声明一个存储顶点索引的整数和一个存储当前点的真实坐标的PVector。然后清空扫描点的数组列表，从头开始更新。

void updateObject(int scanWidth, int step)
{
int index;
PVector realWorldPoint;
scanPoints.clear();
scanColors.clear();

为了计算全局坐标点，我们需要知道转盘的当前角度。这个角度是通过把字符串的整数值从它所在的范围映射到360°（2 PI）而得出的（这个个方法等下有详解）。

serialEvent(myPort);
Angle = radians(map(turnAngle, 0, 1023, 0, 360));
//println("angle = " + Angle);
//draw line

在盒子底部绘制一条线，表示旋转。

pushMatrix();
translate(axis.x, axis.y, axis.z);
rotateY(Angle);
line(0, 0, 100, 0);
popMatrix();

运行一个嵌套循环，提取每个点的真实坐标和它的颜色。

int xMin = (int)(kinect.depthWidth() / 2 - scanWidth / 2);
int xMax = (int)(kinect.depthWidth() / 2 + scanWidth / 2);
for(int y = 0; y < kinect.depthHeight(); y += step) {
for(int x = xMin; x < xMax; x += step) {
index = x + (y * kinect.depthWidth());
realWorldPoint = kinect.depthMapRealWorld()[index];
color pointCol = kinect.rgbImage().pixels[index];

如果当前点包含在这个盒子内（在被允许扫描到的范围内），就可以创建出存储这个点的全局坐标的向量。这堆if是不是很酷炫~

if(realWorldPoint.y < (modelHeight + baseHeight) && realWorldPoint.y > baseHeight) {
if(abs(realWorldPoint.x - axis.x) < modelWidth / 2) {
if(realWorldPoint.z < axis.z + modelWidth / 2 && realWorldPoint.z > axis.z - modelWidth / 2) {
PVector rotatedPoint;

坐标系的旋转过程分为两个步骤，首先，我们需要通过转盘中心得到一个向量坐标。轴向量是通过Kinect坐标系定义转盘中心坐标的，所以你只需要从真实点坐标中减去轴向量就可以得到旋转后的向量。然后，我们需要根据转盘当前的角度让点绕着Y轴旋转。最后使用vecRotY()来完成这个转换。

realWorldPoint.z -= axis.z;
realWorldPoint.x -= axis.x;
rotatedPoint = vecRotY(realWorldPoint, Angle);

现在我们已经找到了扫描点的真实坐标，可以添加进数组列表了。

scanPoints.add(rotatedPoint.get());
scanColors.add(new PVector(red(pointCol), green(pointCol), blue(pointCol)));
}
}
}
}
}
}

vecRotY函数返回的是一个通过输入角度，让输入向量旋转产生的PVector。

PVector vecRotY(PVector vecIn, float phi)
{
PVector rotatedVec = new PVector();
rotatedVec.x = vecIn.x * cos(phi) - vecIn.z * sin(phi);
rotatedVec.z = vecIn.x * sin(phi) + vecIn.z * cos(phi);
rotatedVec.y = vecIn.y;
return rotatedVec;
}

keyPressed方法（检测键盘输入方法）

public void keyPressed()
{
switch(key) {
case 'c':
objectPoints.clear();
objectColors.clear();
currentShot = 0;
break;
case 'e':
String stringNum = String.valueOf(dataNum);
char key = stringNum.charAt(0);
exportPly(key);
dataNum ++;
println("save success!");
break;
case 's':
scan();
break;
}
}

s开始扫描

c是清除所有数据

e是导出

serialEvent方法（通信方法）

public void serialEvent(Serial myPort)
{
String inString = myPort.readStringUntil('\n');
if(inString != null) {
//cut space
turnTableAngle = trim(inString);
turnAngle = Integer.parseInt(turnTableAngle);
}
}

这个是用来和Arduino通信的。

String inString = myPort.readStringUntil(‘\n’);

读取数据，直到\n时停止。

trim详见官网。

turnAngle = Integer.parseInt(turnTableAngle);

显式转换，String -> int

exportPly方法（导出方法）

void exportPly(char key)
{
PrintWriter output;
String viewPointFileName;
viewPointFileName = "myPoints" + key + ".ply";
output = createWriter(dataPath(viewPointFileName));

//!!!!
//head file
output.println("ply");
output.println("format ascii 1.0");
output.println("comment This is your Processing ply file");
output.println("element vertex " + (objectPoints.size()-1));
output.println("property float x");
output.println("property float y");
output.println("property float z");
output.println("property uchar red");
output.println("property uchar green");
output.println("property uchar blue");
output.println("end_header");
//!!!!

for(int i = 0; i < objectPoints.size() - 1; i++) {
output.println(
(objectPoints.get(i).x / 1000) + " "
+ (objectPoints.get(i).y / 1000) + " "
+ (objectPoints.get(i).z / 1000) + " "
+ (int) objectColors.get(i).x + " "
+ (int) objectColors.get(i).y + " "
+ (int) objectColors.get(i).z
);
}

output.flush();
output.close();
}

照抄即可，别管那么多。

知道好使就行。