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

本文链接：https://blog.csdn.net/coder_ma_14/article/details/70349295

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