Python example: mrpt_viz_example.py

Builds a 3D scene with lines, boxes, point clouds and models with mrpt.viz.

Modules: mrpt.viz, mrpt.math

  1#!/usr/bin/env python3
  2"""
  3Builds a 3D scene with lines, boxes, point clouds and models with mrpt.viz.
  4
  5Demonstrates:
  6  - Scene + Viewport: top-level scene graph
  7  - CGridPlaneXY, CAxis: floor grid and coordinate axes
  8  - CBox, CSphere, CCylinder: primitive shapes
  9  - CPointCloud / CPointCloudColoured: point clouds from numpy
 10  - CSetOfLines / CSimpleLine: line primitives
 11  - CText / CText3D: text labels
 12  - CEllipsoid3D: uncertainty ellipsoid
 13  - CArrow: 3D arrows
 14  - stock_objects: pre-built sensor / robot models
 15  - CCamera: scene camera control
 16
 17NOTE: This script builds and inspects the scene without opening a display
 18window (use mrpt_gui_example.py for interactive 3D).
 19"""
 20
 21import math, numpy as np
 22from mrpt.viz import (
 23    Scene, Viewport,
 24    CGridPlaneXY, CAxis,
 25    CBox, CSphere, CCylinder, CArrow,
 26    CText, CText3D,
 27    CSetOfLines, CSimpleLine,
 28    CEllipsoid3D,
 29    CPointCloud, CPointCloudColoured,
 30    stock_objects,
 31)
 32from mrpt.math import TPoint3D, TSegment3D, CMatrixDouble33
 33
 34# ---------------------------------------------------------------------------
 35# Scene and viewport
 36# ---------------------------------------------------------------------------
 37scene = Scene()
 38vp = scene.getViewport("main")
 39print(f"Scene created, viewport: {vp.name}")
 40
 41# Camera setup
 42cam = vp.getCamera()
 43cam.setAzimuthDegrees(45)
 44cam.setElevationDegrees(30)
 45cam.setZoomDistance(10)
 46
 47# ---------------------------------------------------------------------------
 48# Floor grid and axes
 49# ---------------------------------------------------------------------------
 50grid = CGridPlaneXY(-10, 10, -10, 10, 0, 1)
 51scene.insert(grid)
 52
 53axes = CAxis(-3, -3, -3, 3, 3, 3, 1.0, 2.0, True)
 54scene.insert(axes)
 55
 56# ---------------------------------------------------------------------------
 57# Primitive shapes
 58# ---------------------------------------------------------------------------
 59box = CBox(TPoint3D(-0.5, -0.5, 0), TPoint3D(0.5, 0.5, 1.0), False)
 60box.setColor(200, 100, 0)
 61scene.insert(box)
 62
 63sphere = CSphere(0.5, 20)
 64sphere.setLocation(2.0, 0.0, 0.0)
 65sphere.setColor(0, 180, 255)
 66scene.insert(sphere)
 67
 68cyl = CCylinder(0.2, 0.0, 1.5, 20)   # cone shape
 69cyl.setLocation(-2.0, 0.0, 0.0)
 70cyl.setColor(50, 200, 50)
 71scene.insert(cyl)
 72
 73# ---------------------------------------------------------------------------
 74# Arrow
 75# ---------------------------------------------------------------------------
 76arrow = CArrow()
 77arrow.setArrowEnds(0, 0, 1.5, 0, 0, 2.5)
 78arrow.setColor(255, 0, 0)
 79scene.insert(arrow)
 80
 81# ---------------------------------------------------------------------------
 82# Point clouds
 83# ---------------------------------------------------------------------------
 84# Plain XYZ cloud
 85pc = CPointCloud()
 86for i in range(50):
 87    angle = 2 * math.pi * i / 50
 88    pc.insertPoint(3 * math.cos(angle), 3 * math.sin(angle), 0.0)
 89scene.insert(pc)
 90
 91# Coloured cloud
 92pcc = CPointCloudColoured()
 93for i in range(100):
 94    angle = 2 * math.pi * i / 100
 95    r = 2.0
 96    pcc.push_back(r * math.cos(angle), r * math.sin(angle), math.sin(2*angle),
 97                  abs(math.cos(angle)), abs(math.sin(angle)), 0.5)
 98print(f"  CPointCloudColoured: {len(pcc)} points")
 99scene.insert(pcc)
100
101# ---------------------------------------------------------------------------
102# Lines
103# ---------------------------------------------------------------------------
104lines = CSetOfLines()
105lines.appendLine(0, 0, 0, 1, 1, 1)
106lines.appendLine(1, 1, 1, 2, 0, 0)
107lines.appendLine(TSegment3D(TPoint3D(0,0,0), TPoint3D(-1,-1,1)))
108print(f"  CSetOfLines: {len(lines)} segments")
109scene.insert(lines)
110
111simple_line = CSimpleLine()
112simple_line.setLineCoords(-3, 0, 0, 3, 0, 2)
113scene.insert(simple_line)
114
115# ---------------------------------------------------------------------------
116# Text labels
117# ---------------------------------------------------------------------------
118lbl = CText("Hello MRPT")
119lbl.setFont("sans", 20)
120scene.insert(lbl)
121
122lbl3 = CText3D("3D label", "sans", 0.3)
123lbl3.setLocation(1.0, 1.0, 0.0)  # note: "lbl3.pose.x = ..." would modify a copy
124scene.insert(lbl3)
125
126# ---------------------------------------------------------------------------
127# Uncertainty ellipsoid
128# ---------------------------------------------------------------------------
129cov2 = CMatrixDouble33(np.diag([0.5, 0.2, 0.1]))
130ell = CEllipsoid3D()
131ell.setCovMatrix(cov2)
132ell.setQuantiles(2.0)
133ell.set3DsegmentsCount(20)
134ell.setLocation(-2.0, 2.0, 0.0)
135scene.insert(ell)
136
137# ---------------------------------------------------------------------------
138# Stock objects
139# ---------------------------------------------------------------------------
140corner = stock_objects.CornerXYZSimple(0.5, 2.0)
141scene.insert(corner)
142
143pioneer = stock_objects.RobotPioneer()
144scene.insert(pioneer)
145
146hokuyo = stock_objects.Hokuyo_URG()
147scene.insert(hokuyo)
148
149print(f"\nScene built successfully with {len([grid, axes, box, sphere, cyl, arrow, pc, pcc, lines, simple_line, lbl, lbl3, ell, corner, pioneer, hokuyo])} objects")
150print("  (use mrpt_gui_example.py to display interactively)")