|
|
|
|
@ -7,12 +7,18 @@ import org.opencv.core.Core;
|
|
|
|
|
import org.opencv.core.CvType;
|
|
|
|
|
import org.opencv.core.Mat;
|
|
|
|
|
import org.opencv.highgui.HighGui;
|
|
|
|
|
import org.opencv.imgcodecs.Imgcodecs;
|
|
|
|
|
import org.opencv.imgproc.Imgproc;
|
|
|
|
|
|
|
|
|
|
import javax.imageio.ImageIO;
|
|
|
|
|
import java.awt.*;
|
|
|
|
|
import java.awt.image.BufferedImage;
|
|
|
|
|
import java.io.*;
|
|
|
|
|
import java.nio.ByteBuffer;
|
|
|
|
|
import java.nio.ByteOrder;
|
|
|
|
|
import java.text.DecimalFormat;
|
|
|
|
|
import java.util.*;
|
|
|
|
|
import java.util.List;
|
|
|
|
|
import java.util.stream.Collectors;
|
|
|
|
|
|
|
|
|
|
public class PointCloudProcessor {
|
|
|
|
|
@ -277,30 +283,40 @@ public class PointCloudProcessor {
|
|
|
|
|
}
|
|
|
|
|
return points;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private static List<double[]> readBinaryPCD(String filePath) throws IOException {
|
|
|
|
|
public static List<double[]> readBinaryPCD(String filePath) {
|
|
|
|
|
List<double[]> points = new ArrayList<>();
|
|
|
|
|
try (FileInputStream fis = new FileInputStream(filePath)) {
|
|
|
|
|
ByteBuffer buffer = null;
|
|
|
|
|
buffer.order(ByteOrder.LITTLE_ENDIAN);
|
|
|
|
|
// Skip header (assuming header size is known, or read until DATA section)
|
|
|
|
|
// This example assumes 11 lines of header, which may vary
|
|
|
|
|
for (int i = 0; i < 11; i++) {
|
|
|
|
|
while (buffer.get() != '\n') {
|
|
|
|
|
// Skip header lines
|
|
|
|
|
try (DataInputStream dis = new DataInputStream(new FileInputStream(filePath))) {
|
|
|
|
|
// 读取头部信息
|
|
|
|
|
String line;
|
|
|
|
|
Map<String, String> header = new HashMap<>();
|
|
|
|
|
while (!(line = dis.readLine()).equals("DATA binary")) {
|
|
|
|
|
String[] parts = line.split(" ");
|
|
|
|
|
if (parts.length > 1) {
|
|
|
|
|
header.put(parts[0], parts[1]);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
// Read binary data
|
|
|
|
|
while (buffer.hasRemaining()) {
|
|
|
|
|
double x = buffer.getFloat();
|
|
|
|
|
double y = buffer.getFloat();
|
|
|
|
|
double z = buffer.getFloat();
|
|
|
|
|
int rgb = buffer.getInt(); // Read 4 bytes for RGB
|
|
|
|
|
double[] point = {x, y, z};
|
|
|
|
|
|
|
|
|
|
// 获取点云的总点数
|
|
|
|
|
int pointCount = Integer.parseInt(header.get("POINTS"));
|
|
|
|
|
|
|
|
|
|
// 读取二进制数据
|
|
|
|
|
for (int i = 0; i < pointCount; i++) {
|
|
|
|
|
double[] point = new double[3]; // 存储 x, y, z 坐标
|
|
|
|
|
point[0] = dis.readFloat(); // 读取 x
|
|
|
|
|
point[1] = dis.readFloat(); // 读取 y
|
|
|
|
|
point[2] = dis.readFloat(); // 读取 z
|
|
|
|
|
int rgb = dis.readInt(); // 读取 RGB 值（无符号整数）
|
|
|
|
|
|
|
|
|
|
// 可选：解析 RGB，如果需要的话
|
|
|
|
|
// int r = (rgb >> 16) & 0xFF;
|
|
|
|
|
// int g = (rgb >> 8) & 0xFF;
|
|
|
|
|
// int b = rgb & 0xFF;
|
|
|
|
|
|
|
|
|
|
// 你可以存储这些颜色信息，也可以仅处理坐标
|
|
|
|
|
points.add(point);
|
|
|
|
|
}
|
|
|
|
|
} catch (Exception e) {
|
|
|
|
|
return null;
|
|
|
|
|
} catch (IOException e) {
|
|
|
|
|
e.printStackTrace();
|
|
|
|
|
}
|
|
|
|
|
return points;
|
|
|
|
|
}
|
|
|
|
|
@ -419,19 +435,165 @@ public class PointCloudProcessor {
|
|
|
|
|
|
|
|
|
|
System.load(new File(System.getProperty("user.dir")+"\\libs\\opencv\\opencv_java480.dll").getAbsolutePath());
|
|
|
|
|
PcdPojo pojo = new PcdPojo();
|
|
|
|
|
pojo.setPcd1("E:\\pr3d\\03bcfe74-2b4b-41e5-a9f1-09d60090f01b.pcd");
|
|
|
|
|
pojo.setConfigPath("E:\\pr3d\\2-pz-001.json");
|
|
|
|
|
pojo.setPcd1("E:\\1\\1ij.pcd");
|
|
|
|
|
pojo.setConfigPath("E:\\工作\\淮阴\\pz6.json");
|
|
|
|
|
try {
|
|
|
|
|
pojo.setPCDInfo(pojo);
|
|
|
|
|
} catch (IOException e) {
|
|
|
|
|
throw new RuntimeException(e);
|
|
|
|
|
}
|
|
|
|
|
PointCloudProcessor pointCloudProcessor = new PointCloudProcessor();
|
|
|
|
|
List<BoxPositionConf> confs = pointCloudProcessor.getBoxPositionConf("E:\\pr3d\\2-peconf-001.json");
|
|
|
|
|
int i = pointCloudProcessor.slicing(pojo, confs);
|
|
|
|
|
System.out.println(pojo.toString());
|
|
|
|
|
System.out.println(i);
|
|
|
|
|
try {
|
|
|
|
|
saveImage(pojo, "E:\\工作\\淮阴\\03bcfe74-2b4b-41e5-a9f1-09d60090f01b.png");
|
|
|
|
|
} catch (Exception e) {
|
|
|
|
|
throw new RuntimeException(e);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 判断有无
|
|
|
|
|
*
|
|
|
|
|
* @param pojo
|
|
|
|
|
*/
|
|
|
|
|
public boolean judge(PcdPojo pojo) {
|
|
|
|
|
|
|
|
|
|
// 计算pcd1
|
|
|
|
|
List<double[]> points = readPCD(pojo.getPcd1());
|
|
|
|
|
if (points == null || points.isEmpty()) {
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//map xyz
|
|
|
|
|
List<double[]> clippedPoint = points.stream()
|
|
|
|
|
.map(point -> {
|
|
|
|
|
//旋转
|
|
|
|
|
return rotatePoints(point, pojo.getRotationMatrix());
|
|
|
|
|
|
|
|
|
|
})
|
|
|
|
|
.filter(point ->
|
|
|
|
|
//截取
|
|
|
|
|
clipPoints(point, pojo.getMinBounds(), pojo.getMaxBounds())
|
|
|
|
|
)
|
|
|
|
|
.collect(Collectors.toList());
|
|
|
|
|
|
|
|
|
|
painting(points);
|
|
|
|
|
return clippedPoint.size()>500;
|
|
|
|
|
}
|
|
|
|
|
public static BufferedImage createImageFromPointCloud(List<double[]> pointCloud) {
|
|
|
|
|
// Step 1: 计算 x, y, z 的最大值和最小值
|
|
|
|
|
double xMin = Double.MAX_VALUE, xMax = Double.MIN_VALUE;
|
|
|
|
|
double yMin = Double.MAX_VALUE, yMax = Double.MIN_VALUE;
|
|
|
|
|
double zMin = Double.MAX_VALUE, zMax = Double.MIN_VALUE;
|
|
|
|
|
|
|
|
|
|
for (double[] point : pointCloud) {
|
|
|
|
|
double x = point[0];
|
|
|
|
|
double y = point[1];
|
|
|
|
|
double z = point[2];
|
|
|
|
|
|
|
|
|
|
xMin = Math.min(xMin, x);
|
|
|
|
|
xMax = Math.max(xMax, x);
|
|
|
|
|
yMin = Math.min(yMin, y);
|
|
|
|
|
yMax = Math.max(yMax, y);
|
|
|
|
|
zMin = Math.min(zMin, z);
|
|
|
|
|
zMax = Math.max(zMax, z);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Step 2: 计算缩放比例
|
|
|
|
|
double scaleX = 1.0 / (xMax - xMin);
|
|
|
|
|
double scaleY = 1.0 / (yMax - yMin);
|
|
|
|
|
double scaleZ = 1.0 / (zMax - zMin);
|
|
|
|
|
|
|
|
|
|
// 设定图像的大小
|
|
|
|
|
int width = 800; // 假设图像宽度为800
|
|
|
|
|
int height = 600; // 假设图像高度为600
|
|
|
|
|
|
|
|
|
|
// 创建一个二维数组来存储每个像素位置的最小 z 值和对应的颜色
|
|
|
|
|
double[][] minZValues = new double[width][height];
|
|
|
|
|
Color[][] pixelColors = new Color[width][height];
|
|
|
|
|
|
|
|
|
|
// 初始化 minZValues 为最大值
|
|
|
|
|
for (int x = 0; x < width; x++) {
|
|
|
|
|
for (int y = 0; y < height; y++) {
|
|
|
|
|
minZValues[x][y] = Double.MAX_VALUE;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 点的大小（半径）
|
|
|
|
|
int pointRadius = 3; // 改为需要的大小
|
|
|
|
|
|
|
|
|
|
// Step 3: 为每个点分配一个像素位置，并记录最小 z 值
|
|
|
|
|
for (double[] point : pointCloud) {
|
|
|
|
|
// 缩放点的坐标，保留 7% 边距
|
|
|
|
|
int x = (int) ((point[0] - xMin) * (width * 0.93) * scaleX) + (int) (width * 0.035);
|
|
|
|
|
int y = (int) ((point[1] - yMin) * (height * 0.93) * scaleY) + (int) (height * 0.035);
|
|
|
|
|
|
|
|
|
|
// 确保坐标在图像范围内
|
|
|
|
|
if (x >= 0 && x < width && y >= 0 && y < height) {
|
|
|
|
|
// 如果当前点的 z 值更小，则更新
|
|
|
|
|
if (point[2] < minZValues[x][y]) {
|
|
|
|
|
minZValues[x][y] = point[2];
|
|
|
|
|
// 获取深度颜色
|
|
|
|
|
pixelColors[x][y] = getColorFromDepth(point[2], zMin, zMax);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 创建图像并填充白色背景
|
|
|
|
|
BufferedImage image = new BufferedImage(width, height, BufferedImage.TYPE_INT_RGB);
|
|
|
|
|
Graphics2D g2d = image.createGraphics();
|
|
|
|
|
g2d.setColor(Color.WHITE);
|
|
|
|
|
g2d.fillRect(0, 0, width, height);
|
|
|
|
|
g2d.dispose();
|
|
|
|
|
|
|
|
|
|
// Step 4: 绘制点
|
|
|
|
|
for (int x = 0; x < width; x++) {
|
|
|
|
|
for (int y = 0; y < height; y++) {
|
|
|
|
|
if (minZValues[x][y] != Double.MAX_VALUE && pixelColors[x][y] != null) {
|
|
|
|
|
// 在指定位置绘制圆形点
|
|
|
|
|
for (int dy = -pointRadius; dy <= pointRadius; dy++) {
|
|
|
|
|
for (int dx = -pointRadius; dx <= pointRadius; dx++) {
|
|
|
|
|
int nx = x + dx;
|
|
|
|
|
int ny = y + dy;
|
|
|
|
|
if (nx >= 0 && nx < width && ny >= 0 && ny < height) {
|
|
|
|
|
// 如果是圆形区域内
|
|
|
|
|
if (dx*dx + dy*dy <= pointRadius*pointRadius) {
|
|
|
|
|
image.setRGB(nx, ny, pixelColors[x][y].getRGB());
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return image;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private static Color getColorFromDepth(double z, double zMin, double zMax) {
|
|
|
|
|
// 将 z 深度值映射到 0 到 1 范围
|
|
|
|
|
double normalizedDepth = (z - zMin) / (zMax - zMin);
|
|
|
|
|
|
|
|
|
|
// 根据深度值生成 RGB 颜色，从蓝色到红色的渐变
|
|
|
|
|
int red = (int) (normalizedDepth * 255);
|
|
|
|
|
int green = 0;
|
|
|
|
|
int blue = (int) ((1 - normalizedDepth) * 255);
|
|
|
|
|
|
|
|
|
|
return new Color(red, green, blue); // 伪彩色：蓝色到红色渐变
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public static void saveImage(PcdPojo pojo, String filePath) {
|
|
|
|
|
|
|
|
|
|
// 计算pcd1
|
|
|
|
|
List<double[]> points = readPCD(pojo.getPcd1());
|
|
|
|
|
|
|
|
|
|
BufferedImage image =createImageFromPointCloud(points);
|
|
|
|
|
try {
|
|
|
|
|
// 使用 ImageIO 将图像保存为文件
|
|
|
|
|
File outputfile = new File(filePath);
|
|
|
|
|
ImageIO.write(image, "PNG", outputfile); // 可以选择 "PNG", "JPEG", "BMP" 等格式
|
|
|
|
|
System.out.println("图像已保存为：" + filePath);
|
|
|
|
|
} catch (IOException e) {
|
|
|
|
|
System.err.println("保存图像失败: " + e.getMessage());
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
@ -518,11 +680,12 @@ public class PointCloudProcessor {
|
|
|
|
|
Core.normalize(image, normalizedImage, 0, 255, Core.NORM_MINMAX);
|
|
|
|
|
|
|
|
|
|
// 显示图像
|
|
|
|
|
HighGui.imshow("Depth Image", normalizedImage);
|
|
|
|
|
HighGui.waitKey();
|
|
|
|
|
HighGui.destroyAllWindows();
|
|
|
|
|
// HighGui.imshow("Depth Image", normalizedImage);
|
|
|
|
|
// HighGui.waitKey();
|
|
|
|
|
// HighGui.destroyAllWindows();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
public List<BoxPositionConf> getBoxPositionConf(String path) {
|
|
|
|
|
//读取点云位置
|
|
|
|
|
ObjectMapper mapper = new ObjectMapper();
|
|
|
|
|
|
LAPTOP-S9HJSOEB\昊天
7 months ago