|
@@ -0,0 +1,192 @@
|
|
|
+package com.fdkankan.indoor.base.convert;
|
|
|
+
|
|
|
+import com.fdkankan.indoor.base.constant.MsgCode;
|
|
|
+import com.fdkankan.indoor.base.exception.BaseRuntimeException;
|
|
|
+import com.fdkankan.indoor.core.entity.ControlPointEntity;
|
|
|
+import lombok.extern.slf4j.Slf4j;
|
|
|
+import net.sf.json.JSONObject;
|
|
|
+
|
|
|
+import java.util.ArrayList;
|
|
|
+import java.util.Arrays;
|
|
|
+import java.util.List;
|
|
|
+
|
|
|
+/**
|
|
|
+ * Created by owen on 2021/8/19 0019 15:26
|
|
|
+ * 修改数据包含:final_freespace.csv
|
|
|
+ */
|
|
|
+@Slf4j
|
|
|
+public class FixRouteMap {
|
|
|
+
|
|
|
+ private static double[] getTransfromMatrix(double orientation, JSONObject location)
|
|
|
+ {
|
|
|
+ double a1 = Math.cos(orientation);
|
|
|
+ double a2 = Math.sin(orientation);
|
|
|
+
|
|
|
+ double[] mat = new double[16];
|
|
|
+ mat[0] = a1;
|
|
|
+ mat[1] = a2;
|
|
|
+ mat[4] = -1*a2;
|
|
|
+ mat[5] = a1;
|
|
|
+ mat[10] = 1;
|
|
|
+ mat[12] = location.getDouble("x");
|
|
|
+ mat[13] = location.getDouble("y");
|
|
|
+ mat[14] = location.getDouble("z");
|
|
|
+ mat[15] = 1;
|
|
|
+ return mat;
|
|
|
+ }
|
|
|
+
|
|
|
+ private static double[] applyMatrix4(double[] position, double[] e ) {
|
|
|
+
|
|
|
+ double x = position[0];
|
|
|
+ double y = position[1];
|
|
|
+ double z = position[2];
|
|
|
+ double w = 1 / ( e[ 3 ] * x + e[ 7 ] * y + e[ 11 ] * z + e[ 15 ] );
|
|
|
+
|
|
|
+ position[0] = ( e[ 0 ] * x + e[ 4 ] * y + e[ 8 ] * z + e[ 12 ] ) * w;
|
|
|
+ position[1] = ( e[ 1 ] * x + e[ 5 ] * y + e[ 9 ] * z + e[ 13 ] ) * w;
|
|
|
+ position[2] = ( e[ 2 ] * x + e[ 6 ] * y + e[ 10 ] * z + e[ 14 ] ) * w;
|
|
|
+
|
|
|
+ return position;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * 更新route数据
|
|
|
+ * @param orientation dataSet.orientation
|
|
|
+ * @param gpsLocation dataSet.location
|
|
|
+ * @param controlPoint 对应场景码控制点
|
|
|
+ * @param path final_freespace.csv 文件目录路径
|
|
|
+ * @return
|
|
|
+ * @throws Exception
|
|
|
+ */
|
|
|
+ public static List<String> updateRouteMap(double orientation, double[] gpsLocation, ControlPointEntity controlPoint, String path) throws Exception {
|
|
|
+
|
|
|
+ // dataSet.location的gps坐标转本地坐标
|
|
|
+ double[] ageLocation = GisCoordinateUtil.convertGpsToLocation(gpsLocation, controlPoint);
|
|
|
+
|
|
|
+ // 新的本地坐标, 转出来应该是原点, 很接近0
|
|
|
+ JSONObject newLocation = new JSONObject();
|
|
|
+ newLocation.put("x", ageLocation[0]);
|
|
|
+ newLocation.put("y", ageLocation[1]);
|
|
|
+ newLocation.put("z", gpsLocation[2]);
|
|
|
+ log.info("newLocation: {}", newLocation );
|
|
|
+
|
|
|
+
|
|
|
+ double[] newMatrix = getTransfromMatrix(orientation, newLocation);
|
|
|
+
|
|
|
+
|
|
|
+ // 读取棋盘的坐标,读文件final_freespace.csv,不要读数据库,最初的坐标
|
|
|
+ String routePath = path + "/final_freespace.csv";
|
|
|
+ log.info("routePath: {}", routePath);
|
|
|
+ if (!cn.hutool.core.io.FileUtil.isFile(routePath)){
|
|
|
+ log.info("输入路径有误, path: {}", routePath);
|
|
|
+ throw new BaseRuntimeException(MsgCode.e3001, "final_freespace.csv路径有误,文件不存在");
|
|
|
+ }
|
|
|
+ List<String> list = FileUtil.readFileByLines2(routePath);
|
|
|
+ log.info("数据读取成功");
|
|
|
+
|
|
|
+ // 修改棋盘每个顶点坐标,然后再入库
|
|
|
+ List<String> newRoute = new ArrayList<>();
|
|
|
+ for (String line : list) {
|
|
|
+ // 将每一行的坐标值获取处理进行修改
|
|
|
+ String[] split = line.split(" ");
|
|
|
+
|
|
|
+ double[] oldPoint = {Double.valueOf(split[0]), Double.valueOf(split[1]), Double.valueOf(split[2])};
|
|
|
+ double[] newPoint = applyMatrix4(oldPoint,newMatrix);
|
|
|
+
|
|
|
+ // 重新组装每一行数据
|
|
|
+ String newLine = Arrays.asList(newPoint[0], newPoint[1], newPoint[2], split[3], split[4],
|
|
|
+ split[5], split[6], split[7], split[8], split[9], split[10], split[11]).toString();
|
|
|
+ // 去逗号
|
|
|
+ newLine = newLine.replaceAll(",", "");
|
|
|
+ newLine = newLine.replace("[", "");
|
|
|
+ newLine = newLine.replace("]", "");
|
|
|
+ newRoute.add(newLine);
|
|
|
+ }
|
|
|
+ log.info("数据解析成功");
|
|
|
+ return newRoute;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * 本地测试 修改 Route数据
|
|
|
+ * @param args
|
|
|
+ * @throws Exception
|
|
|
+ */
|
|
|
+ public static void main(String[] args) throws Exception {
|
|
|
+// FixRouteMap fixRouteMap = new FixRouteMap();
|
|
|
+
|
|
|
+ //对应的是datasets表里的orientation
|
|
|
+ double newOrientation = 0;
|
|
|
+
|
|
|
+ //需要转换,将gis坐标转换本地坐标
|
|
|
+ //将datasets表里location的坐标还原成本地坐标
|
|
|
+// double[] gpsLocation = {113.595719800031, 22.3666054020125, 0.0};
|
|
|
+ double[] gpsLocation = {113.59571980003058, 22.36660540201251, 0};
|
|
|
+ // 将 gps 同 age
|
|
|
+
|
|
|
+ // 控制点
|
|
|
+ ControlPointEntity controlPoint = new ControlPointEntity();
|
|
|
+ double[] controlCoordinate1 = {113.595725873337,22.366579193007}; //gps坐标
|
|
|
+ double[] controlCoordinate2 = {113.595757230122,22.3666213677456}; //gps坐标
|
|
|
+
|
|
|
+ double[] controlLocation1 = {0.617759,-2.904041}; // 四维看看坐标
|
|
|
+ double[] controlLocation2 = {3.859914,1.75765}; // 四维看看坐标
|
|
|
+
|
|
|
+
|
|
|
+ controlPoint.setGpsControlCoordinate1(controlCoordinate1);
|
|
|
+ controlPoint.setGpsControlCoordinate2(controlCoordinate2);
|
|
|
+ controlPoint.setAgeControlLocation1(controlLocation1);
|
|
|
+ controlPoint.setAgeControlLocation2(controlLocation2);
|
|
|
+ double[] ageLocation = GisCoordinateUtil.convertGpsToLocation(gpsLocation, controlPoint);
|
|
|
+
|
|
|
+ // 新的本地坐标
|
|
|
+ JSONObject newLocation = new JSONObject();
|
|
|
+ newLocation.put("x", ageLocation[0]);
|
|
|
+ newLocation.put("y", ageLocation[1]);
|
|
|
+ newLocation.put("z", gpsLocation[2]);
|
|
|
+ log.info("ageLocation: {}", newLocation );
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ double[] newMatrix = getTransfromMatrix(newOrientation, newLocation);
|
|
|
+
|
|
|
+
|
|
|
+ // 读取棋盘的坐标,读文件final_freespace.csv,不要读数据库,最初的坐标
|
|
|
+
|
|
|
+ String path = "F:\\test\\project\\age_laser\\laserData";
|
|
|
+ String routePath = path + "/final_freespace.csv";
|
|
|
+ log.info("routePath: {}", routePath);
|
|
|
+ if (!cn.hutool.core.io.FileUtil.isFile(routePath)){
|
|
|
+ log.info("输入路径有误, path: {}", routePath);
|
|
|
+ throw new BaseRuntimeException(MsgCode.e3001, "final_freespace.csv路径有误,文件不存在");
|
|
|
+ }
|
|
|
+ List<String> list = FileUtil.readFileByLines2(routePath);
|
|
|
+
|
|
|
+ // 修改棋盘每个顶点坐标,然后再入库
|
|
|
+ List<String> newRoute = new ArrayList<>();
|
|
|
+ for (String line : list) {
|
|
|
+ // 将每一行的坐标值获取处理进行修改
|
|
|
+ String[] split = line.split(" ");
|
|
|
+
|
|
|
+ double[] oldPoint = {Double.valueOf(split[0]), Double.valueOf(split[1]), Double.valueOf(split[2])};
|
|
|
+ double[] newPoint = applyMatrix4(oldPoint,newMatrix); //修改棋盘每个顶点坐标,然后再入库
|
|
|
+
|
|
|
+ String newLine = Arrays.asList(newPoint[0], newPoint[1], newPoint[2], split[3], split[4],
|
|
|
+ split[5], split[6], split[7], split[8], split[9], split[10], split[11]).toString();
|
|
|
+
|
|
|
+ // 去逗号
|
|
|
+ newLine = newLine.replaceAll(",", "");
|
|
|
+ newLine = newLine.replace("[", "");
|
|
|
+ newLine = newLine.replace("]", "");
|
|
|
+ System.out.println(newLine);
|
|
|
+ newRoute.add(newLine);
|
|
|
+ }
|
|
|
+
|
|
|
+ System.out.println("执行完成");
|
|
|
+
|
|
|
+
|
|
|
+// double[] oldPoint = new double[3]; //
|
|
|
+// double[] newPoint = fixRouteMap.applyMatrix4(oldPoint,newMatrix); //修改棋盘每个顶点坐标,然后再入库
|
|
|
+ }
|
|
|
+}
|