Merge pull request #331 from open-mmlab/feat/pointcloud-fisheye-polygon

Feat/pointcloud fisheye polygon
open-mmlab · Oct 16, 2023 · 6960583 · 6960583
2 parents ab6ebb8 + 3baa286
commit 6960583
Show file tree

Hide file tree

Showing 4 changed files with 108 additions and 6 deletions.
diff --git a/packages/lb-annotation/src/core/pointCloud/matrix.ts b/packages/lb-annotation/src/core/pointCloud/matrix.ts
@@ -14,6 +14,7 @@ import {
   IPolygonPoint,
   ICalib,
   IBasicBox3d,
+  ICoordinate,
 } from '@labelbee/lb-utils';
 import uuid from '@/utils/uuid';
 
@@ -484,3 +485,60 @@ export function pointMappingLidar2image(
   }
   return { pcdMapping };
 }
+
+export function pointListLidar2Img(
+  pointList3D: I3DSpaceCoord[],
+  calib?: ICalib,
+  filterSize?: {
+    width: number;
+    height: number;
+  },
+) {
+  if (!calib || !filterSize) {
+    return;
+  }
+
+  const isFisheyeCalib = isFisheyeCalibValid(calib);
+
+  const { P, R, T } = calib;
+  let composeMatrix4: THREE.Matrix4 | undefined;
+
+  /**
+   * 1. Default pattern initialize composeMatrix4
+   *
+   * Avoid double counting
+   */
+  if (isFisheyeCalib === false) {
+    const { composeMatrix4: matrix4 } = transferKitti2Matrix(P, R, T) ?? {};
+    if (!matrix4) {
+      return;
+    }
+
+    composeMatrix4 = matrix4;
+  }
+  const pointList2D: Array<ICoordinate> = [];
+
+  // 2. Transform pointList3D
+  pointList3D.forEach((point3D) => {
+    let point2d;
+    if (isFisheyeCalib) {
+      point2d = lidar2FisheyeImage(point3D, calib);
+    } else {
+      point2d = composeMatrix4 && lidar2image(point3D, composeMatrix4);
+    }
+
+    if (point2d) {
+      const { x, y } = point2d;
+
+      pointList2D.push({ x, y });
+    }
+  });
+  // 有一个点在图像内就返回整个多边形
+  const hasInImagePoint = pointList2D.some((point) => {
+    return point.x > 0 && point.x < filterSize.width && point.y > 0 && point.y < filterSize.height;
+  });
+
+  if (hasInImagePoint) {
+    return pointList2D;
+  }
+}
diff --git a/packages/lb-components/src/components/pointCloudView/PointCloud2DView.tsx b/packages/lb-components/src/components/pointCloudView/PointCloud2DView.tsx
@@ -4,7 +4,7 @@ import { PointCloudContainer } from './PointCloudLayout';
 import { PointCloudContext } from './PointCloudContext';
 import { connect } from 'react-redux';
 
-import { pointCloudLidar2image, cKeyCode } from '@labelbee/lb-annotation';
+import { pointCloudLidar2image, cKeyCode, pointListLidar2Img } from '@labelbee/lb-annotation';
 import { LabelBeeContext } from '@/store/ctx';
 import { a2MapStateToProps, IA2MapStateProps } from '@/store/annotation/map';
 import { ICalib, IPointCloudBox, IPolygonPoint, toolStyleConverter } from '@labelbee/lb-utils';
@@ -29,6 +29,8 @@ export interface IAnnotationDataTemporarily {
   };
 }
 
+const DEFAULT_GROUND_HEIGHT = -1.345;
+
 interface ITransferViewData {
   type: string;
   pointList: {
@@ -120,7 +122,8 @@ const PointCloud2DView = ({
   checkMode,
 }: IProps) => {
   const [annotations2d, setAnnotations2d] = useState<IAnnotationData2dView[]>([]);
-  const { topViewInstance, displayPointCloudList } = useContext(PointCloudContext);
+  const { topViewInstance, displayPointCloudList, polygonList, imageSizes, selectedIDs } =
+    useContext(PointCloudContext);
   const [selectedID, setSelectedID] = useState<number | string>('');
   const [isEnlarge, setIsEnlarge] = useState<boolean>(false);
   const [curIndex, setCurIndex] = useState<number | undefined>(undefined);
@@ -189,6 +192,44 @@ const PointCloud2DView = ({
           },
           [],
         );
+
+        const imageSize = imageSizes[mappingData?.path ?? ''];
+
+        if (imageSize) {
+          polygonList.forEach((polygon) => {
+            // eslint-disable-next-line
+            const polygonPoints = polygon.pointList.map((v) => ({
+              ...v,
+              z: mappingData?.calib?.groundHeight ?? DEFAULT_GROUND_HEIGHT,
+            }));
+            const result = pointListLidar2Img(polygonPoints, mappingData?.calib, imageSize);
+
+            if (result) {
+              const polygonColor = toolStyleConverter.getColorFromConfig(
+                { attribute: polygon.attribute },
+                {
+                  ...config,
+                  attributeConfigurable: true,
+                },
+                {},
+              );
+
+              newAnnotations2d.push({
+                type: 'polygon',
+                annotation: {
+                  id: polygon.id,
+                  pointList: result,
+                  ...defaultViewStyle,
+                  stroke: polygonColor?.stroke,
+                  fill: selectedIDs.includes(polygon.id)
+                    ? polygonColor?.fill
+                    : 'rgba(255, 255, 255, 0.6)',
+                },
+              });
+            }
+          });
+        }
+
         newAnnotations2dList.push({
           annotations: newAnnotations2d,
           url: mappingData?.url,
@@ -205,6 +246,9 @@ const PointCloud2DView = ({
     selectedID,
     highlightAttribute,
     loadPCDFileLoading,
+    polygonList,
+    imageSizes,
+    selectedIDs,
   ]);
 
   useEffect(() => {

diff --git a/packages/lb-components/src/components/pointCloudView/hooks/usePointCloudViews.ts b/packages/lb-components/src/components/pointCloudView/hooks/usePointCloudViews.ts
@@ -1046,12 +1046,12 @@ export const usePointCloudViews = () => {
       /**
        * Notice. The Polygon need to be converted to pointCloud coordinate system for storage.
        */
-      const polygon = updateList[0].newPolygon;
+      const polygon = { ...updateList[0].newPolygon };
       polygon.pointList = polygon.pointList.map((v) =>
         PointCloudUtils.transferCanvas2World(v, size),
       );
 
-      pushHistoryUnderUpdatePolygon(updateList[0].newPolygon);
+      pushHistoryUnderUpdatePolygon(polygon);
       return;
     }
 

diff --git a/packages/lb-utils/src/types/pointCloud.ts b/packages/lb-utils/src/types/pointCloud.ts
@@ -128,15 +128,15 @@ interface ICalib {
   P: [TMatrix14Tuple, TMatrix14Tuple, TMatrix14Tuple]; // 3x4 Camera Intrinsic matrix
   R: [TMatrix13Tuple, TMatrix13Tuple, TMatrix13Tuple]; // 3x3 rotation matrix
   T: [TMatrix14Tuple, TMatrix14Tuple, TMatrix14Tuple]; // 3x4 Lidar to camera matrix
-
+  groundHeight?: number; // Ground height. 地面高度。
   calName?: string; // Camera Name
 }
 
 interface ICalib {
   P: [TMatrix14Tuple, TMatrix14Tuple, TMatrix14Tuple]; // 3x4 Camera Intrinsic matrix
   T: [TMatrix14Tuple, TMatrix14Tuple, TMatrix14Tuple]; // 3x4 Lidar to camera matrix
   fisheyeDistortion: number[]; // Omnidirectional camera: fisheye distortion. 全方向摄像机 - 鱼眼畸变参数。
-
+  groundHeight?: number; // Ground height. 地面高度。
   calName?: string; // Camera Name
 }