mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-23 01:21:40 +00:00
Compare commits
8 Commits
v2025.3.1-
...
v2025.3.1
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
4e1d7bbb3d | ||
|
|
97dbcdd252 | ||
|
|
0ef7c803f9 | ||
|
|
410a4c75b7 | ||
|
|
edf42f5102 | ||
|
|
3e879cc30f | ||
|
|
c2127ac820 | ||
|
|
002373e395 |
@@ -1,10 +1,10 @@
|
||||
# 3D Tracking
|
||||
|
||||
3D AprilTag tracking will allow you to track the real-world position and rotation of a tag relative to the camera's image sensor. This is useful for robot pose estimation and other applications like autonomous scoring. In order to use 3D tracking, you must first {ref}`calibrate your camera <docs/calibration/calibration:Calibrating Your Camera>`. Once you have, you need to enable 3D mode in the UI and you will now be able to get 3D pose information from the tag! For information on getting and using this information in your code, see {ref}`the programming reference. <docs/programming/index:Programming Reference>`.
|
||||
3D AprilTag tracking will allow you to track the real-world position and rotation of a tag relative to the camera's image sensor. This is useful for robot pose estimation and other applications like autonomous scoring. In order to use 3D tracking, you must first {ref}`calibrate your camera <docs/calibration/calibration:Calibrating Your Camera>`. Once you have, you need to enable 3D mode in the UI and you will now be able to get 3D pose information from the tag! For information on getting and using this information in your code, see {ref}`the programming reference <docs/programming/index:Programming Reference>`.
|
||||
|
||||
## Ambiguity
|
||||
|
||||
Translating from 2D to 3D using data from the calibration and the four tag corners can lead to "pose ambiguity", where it appears that the AprilTag pose is flipping between two different poses. You can read more about this issue `here. <https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html#d-to-3d-ambiguity>` Ambiguity is calculated as the ratio of reprojection errors between two pose solutions (if they exist), where reprojection error is the error corresponding to the image distance between where the apriltag's corners are detected vs where we expect to see them based on the tag's estimated camera relative pose.
|
||||
Translating from 2D to 3D using data from the calibration and the four tag corners can lead to "pose ambiguity", where it appears that the AprilTag pose is flipping between two different poses. You can read more about this issue [here](https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html#d-to-3d-ambiguity). Ambiguity is calculated as the ratio of reprojection errors between two pose solutions (if they exist), where reprojection error is the error corresponding to the image distance between where the apriltag's corners are detected vs where we expect to see them based on the tag's estimated camera relative pose.
|
||||
|
||||
There are a few steps you can take to resolve/mitigate this issue:
|
||||
|
||||
|
||||
@@ -69,6 +69,16 @@ In the root directory:
|
||||
``gradlew buildAndCopyUI``
|
||||
```
|
||||
|
||||
### Using hot reload on the UI
|
||||
|
||||
In the photon-client directory:
|
||||
|
||||
```bash
|
||||
npm run dev
|
||||
```
|
||||
|
||||
This allows you to make UI changes quickly without having to spend time rebuilding the jar. Hot reload is enabled, so changes that you make and save are reflected in the UI immediately. Running this command will give you the URL for accessing the UI, which is on a different port than normal. You must use the printed URL to use hot reload.
|
||||
|
||||
### Build and Run PhotonVision
|
||||
|
||||
To compile and run the project, issue the following command in the root directory:
|
||||
|
||||
@@ -11,7 +11,7 @@ When using PhotonVision off robot, you _MUST_ plug the coprocessor into a physic
|
||||
:::{tab-item} New Radio (2025 - present)
|
||||
|
||||
```{danger}
|
||||
Ensure that the radio's DIP switches 1 and 2 are turned off; otherwise, the radio PoE feature may electrically destroy your coprocessor. [More info.](https://frc-radio.vivid-hosting.net/getting-started/passive-power-over-ethernet-poe-for-downstream-devices)
|
||||
Ensure that the radio's DIP switches 1 and 2 are turned off; otherwise, the radio PoE feature may electrically destroy your coprocessor. [More info.](https://frc-radio.vivid-hosting.net/overview/wiring-your-radio#power-over-ethernet-poe-for-downstream-devices)
|
||||
```
|
||||
|
||||
```{image} images/networking-diagram-vividhosting.png
|
||||
|
||||
@@ -170,5 +170,13 @@ const interactiveCols = computed(() =>
|
||||
:select-cols="interactiveCols"
|
||||
@input="(args) => handleStreamResolutionChange(args)"
|
||||
/>
|
||||
<pv-switch
|
||||
v-if="useCameraSettingsStore().isDriverMode"
|
||||
v-model="useCameraSettingsStore().currentPipelineSettings.crosshair"
|
||||
label="Crosshair"
|
||||
:switch-cols="interactiveCols"
|
||||
tooltip="Enables or disables a crosshair overlay on the camera stream"
|
||||
@input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ crosshair: args }, false)"
|
||||
/>
|
||||
</div>
|
||||
</template>
|
||||
|
||||
@@ -108,8 +108,8 @@ const resetCurrentBuffer = () => {
|
||||
<td class="text-center">{{ target.area.toFixed(2) }}°</td>
|
||||
</template>
|
||||
<template v-else>
|
||||
<td class="text-center">{{ target.pose?.x.toFixed(2) }} m</td>
|
||||
<td class="text-center">{{ target.pose?.y.toFixed(2) }} m</td>
|
||||
<td class="text-center">{{ target.pose?.x.toFixed(3) }} m</td>
|
||||
<td class="text-center">{{ target.pose?.y.toFixed(3) }} m</td>
|
||||
<td class="text-center">{{ toDeg(target.pose?.angle_z || 0).toFixed(2) }}°</td>
|
||||
</template>
|
||||
<template
|
||||
@@ -157,13 +157,13 @@ const resetCurrentBuffer = () => {
|
||||
<tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
|
||||
<tr>
|
||||
<td class="text-center white--text">
|
||||
{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }} m
|
||||
{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(3) }} m
|
||||
</td>
|
||||
<td class="text-center white--text">
|
||||
{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }} m
|
||||
{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(3) }} m
|
||||
</td>
|
||||
<td class="text-center white--text">
|
||||
{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(2) }} m
|
||||
{{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(3) }} m
|
||||
</td>
|
||||
<td class="text-center white--text">
|
||||
{{
|
||||
|
||||
@@ -478,7 +478,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
|
||||
const url = new URL(`http://${host}/api/utils/getCalibrationJSON`);
|
||||
url.searchParams.set("width", Math.round(resolution.width).toFixed(0));
|
||||
url.searchParams.set("height", Math.round(resolution.height).toFixed(0));
|
||||
url.searchParams.set("cameraUniqueName", cameraUniqueName.replace(" ", "").trim().toLowerCase());
|
||||
url.searchParams.set("cameraUniqueName", cameraUniqueName);
|
||||
|
||||
return url.href;
|
||||
}
|
||||
|
||||
@@ -84,8 +84,6 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
|
||||
public void accept(CVMat image, Date now) {
|
||||
long currentCount = saveFrameEntry.get();
|
||||
|
||||
System.out.println("currentCount: " + currentCount + " savedImagesCount: " + savedImagesCount);
|
||||
|
||||
// Await save request
|
||||
if (currentCount == -1) return;
|
||||
|
||||
|
||||
@@ -68,10 +68,12 @@ public class DriverModePipeline
|
||||
if (!emptyIn) {
|
||||
totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
|
||||
|
||||
var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
|
||||
if (settings.crosshair) {
|
||||
var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
|
||||
|
||||
// calculate elapsed nanoseconds
|
||||
totalNanos += draw2dCrosshairResult.nanosElapsed;
|
||||
// calculate elapsed nanoseconds
|
||||
totalNanos += draw2dCrosshairResult.nanosElapsed;
|
||||
}
|
||||
}
|
||||
|
||||
var fpsResult = calculateFPSPipe.run(null);
|
||||
@@ -92,6 +94,7 @@ public class DriverModePipeline
|
||||
|
||||
@Override
|
||||
public void release() {
|
||||
// we never actually need to give resources up since pipelinemanager only makes one of us
|
||||
// we never actually need to give resources up since pipelinemanager only makes
|
||||
// one of us
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,6 +24,7 @@ import org.photonvision.vision.processes.PipelineManager;
|
||||
@JsonTypeName("DriverModePipelineSettings")
|
||||
public class DriverModePipelineSettings extends CVPipelineSettings {
|
||||
public DoubleCouple offsetPoint = new DoubleCouple();
|
||||
public boolean crosshair = true;
|
||||
|
||||
public DriverModePipelineSettings() {
|
||||
super();
|
||||
|
||||
@@ -57,6 +57,31 @@ static void ClientLoggerFunc(unsigned int level, const char* file,
|
||||
line);
|
||||
}
|
||||
|
||||
void wpi::tsp::TimeSyncClient::UpdateStatistics(uint64_t pong_local_time,
|
||||
wpi::tsp::TspPing ping,
|
||||
wpi::tsp::TspPong pong) {
|
||||
// when time = send_time+rtt2/2, server time = server time
|
||||
// server time = local time + offset
|
||||
// offset = (server time - local time) = (server time) - (send_time +
|
||||
// rtt2/2)
|
||||
auto rtt2 = pong_local_time - ping.client_time;
|
||||
int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
|
||||
|
||||
auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs);
|
||||
|
||||
// wpi::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS",
|
||||
// rtt2,
|
||||
// serverTimeOffsetUs, filtered);
|
||||
|
||||
{
|
||||
std::lock_guard lock{m_offsetMutex};
|
||||
m_metadata.offset = filtered;
|
||||
m_metadata.rtt2 = rtt2;
|
||||
m_metadata.pongsReceived++;
|
||||
m_metadata.lastPongTime = pong_local_time;
|
||||
}
|
||||
}
|
||||
|
||||
void wpi::tsp::TimeSyncClient::Tick() {
|
||||
// wpi::println("wpi::tsp::TimeSyncClient::Tick");
|
||||
// Regardless of if we've gotten a pong back yet, we'll ping again. this is
|
||||
@@ -122,28 +147,9 @@ void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes,
|
||||
return;
|
||||
}
|
||||
|
||||
// when time = send_time+rtt2/2, server time = server time
|
||||
// server time = local time + offset
|
||||
// offset = (server time - local time) = (server time) - (send_time +
|
||||
// rtt2/2)
|
||||
auto rtt2 = pong_local_time - ping.client_time;
|
||||
int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
|
||||
UpdateStatistics(pong_local_time, ping, pong);
|
||||
|
||||
auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs);
|
||||
|
||||
// wpi::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS",
|
||||
// rtt2,
|
||||
// serverTimeOffsetUs, filtered);
|
||||
|
||||
{
|
||||
std::lock_guard lock{m_offsetMutex};
|
||||
m_metadata.offset = filtered;
|
||||
m_metadata.rtt2 = rtt2;
|
||||
m_metadata.pongsReceived++;
|
||||
m_metadata.lastPongTime = pong_local_time;
|
||||
}
|
||||
|
||||
using std::cout;
|
||||
// using std::cout;
|
||||
// wpi::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2,
|
||||
// serverTimeOffsetUs);
|
||||
// wpi::println("Estimated server time {} s",
|
||||
|
||||
@@ -96,6 +96,10 @@ class TimeSyncClient {
|
||||
void Stop();
|
||||
int64_t GetOffset();
|
||||
Metadata GetMetadata();
|
||||
|
||||
// public for testability
|
||||
void UpdateStatistics(uint64_t pong_local_time, wpi::tsp::TspPing ping,
|
||||
wpi::tsp::TspPong pong);
|
||||
};
|
||||
|
||||
} // namespace tsp
|
||||
|
||||
@@ -38,3 +38,115 @@ TEST(TimeSyncProtocolTest, Smoketest) {
|
||||
|
||||
server.Stop();
|
||||
}
|
||||
|
||||
TEST(TimeSyncClientTest, CalculateZero) {
|
||||
using namespace wpi::tsp;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// GIVEN a fresh client
|
||||
TimeSyncClient client{"127.0.0.1", 5812, 100ms};
|
||||
|
||||
// AND a ping-pong sent with no delay
|
||||
// client -> server -> client
|
||||
uint64_t ping_client_time{100};
|
||||
uint64_t pong_server_time{100};
|
||||
uint64_t pong_client_time{100};
|
||||
|
||||
// setup our ping/pong packets
|
||||
TspPing ping{.version = 1, .message_id = 1, .client_time = ping_client_time};
|
||||
TspPong pong{ping, pong_server_time};
|
||||
|
||||
// WHEN we update statistics
|
||||
client.UpdateStatistics(pong_client_time, ping, pong);
|
||||
|
||||
// THEN the statistics will reflect no delay
|
||||
EXPECT_EQ(0, client.GetMetadata().offset);
|
||||
EXPECT_EQ(0, client.GetMetadata().rtt2);
|
||||
EXPECT_EQ(1u, client.GetMetadata().pongsReceived);
|
||||
EXPECT_EQ(pong_client_time, client.GetMetadata().lastPongTime);
|
||||
}
|
||||
|
||||
TEST(TimeSyncClientTest, CalculateZeroOffset) {
|
||||
using namespace wpi::tsp;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// GIVEN a fresh client
|
||||
TimeSyncClient client{"127.0.0.1", 5812, 100ms};
|
||||
|
||||
// AND a ping-pong sent with 10ms delay each way
|
||||
// client -> server -> client
|
||||
uint64_t ping_client_time{100};
|
||||
uint64_t pong_server_time{110};
|
||||
uint64_t pong_client_time{120};
|
||||
|
||||
// setup our ping/pong packets
|
||||
TspPing ping{.version = 1, .message_id = 1, .client_time = ping_client_time};
|
||||
TspPong pong{ping, pong_server_time};
|
||||
|
||||
// WHEN we update statistics
|
||||
client.UpdateStatistics(pong_client_time, ping, pong);
|
||||
|
||||
// THEN the statistics will reflect no offset, and the expected rtt2
|
||||
// (client-to-client) latency
|
||||
EXPECT_EQ(0, client.GetMetadata().offset);
|
||||
EXPECT_EQ(20, client.GetMetadata().rtt2);
|
||||
EXPECT_EQ(1u, client.GetMetadata().pongsReceived);
|
||||
EXPECT_EQ(pong_client_time, client.GetMetadata().lastPongTime);
|
||||
}
|
||||
|
||||
TEST(TimeSyncClientTest, CalculateZeroRtt) {
|
||||
using namespace wpi::tsp;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// GIVEN a fresh client
|
||||
TimeSyncClient client{"127.0.0.1", 5812, 100ms};
|
||||
|
||||
// AND a ping-pong sent with no delay
|
||||
// client -> server -> client
|
||||
uint64_t ping_client_time{100};
|
||||
uint64_t pong_server_time{123};
|
||||
uint64_t pong_client_time{100};
|
||||
|
||||
// setup our ping/pong packets
|
||||
TspPing ping{.version = 1, .message_id = 1, .client_time = ping_client_time};
|
||||
TspPong pong{ping, pong_server_time};
|
||||
|
||||
// WHEN we update statistics
|
||||
client.UpdateStatistics(pong_client_time, ping, pong);
|
||||
|
||||
// THEN the statistics will reflect the expected 23ms offset
|
||||
EXPECT_EQ(23, client.GetMetadata().offset);
|
||||
EXPECT_EQ(0, client.GetMetadata().rtt2);
|
||||
EXPECT_EQ(1u, client.GetMetadata().pongsReceived);
|
||||
EXPECT_EQ(pong_client_time, client.GetMetadata().lastPongTime);
|
||||
}
|
||||
|
||||
TEST(TimeSyncClientTest, CalculateBoth) {
|
||||
using namespace wpi::tsp;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// GIVEN a fresh client
|
||||
TimeSyncClient client{"127.0.0.1", 5812, 100ms};
|
||||
|
||||
// AND a ping-pong sent with no delay
|
||||
// client -> server -> client
|
||||
int64_t offset{-234};
|
||||
int64_t network_latency{23};
|
||||
|
||||
uint64_t ping_client_time{100};
|
||||
uint64_t pong_server_time{ping_client_time + offset + network_latency};
|
||||
uint64_t pong_client_time{ping_client_time + 2 * network_latency};
|
||||
|
||||
// setup our ping/pong packets
|
||||
TspPing ping{.version = 1, .message_id = 1, .client_time = ping_client_time};
|
||||
TspPong pong{ping, pong_server_time};
|
||||
|
||||
// WHEN we update statistics
|
||||
client.UpdateStatistics(pong_client_time, ping, pong);
|
||||
|
||||
// THEN the statistics will reflect the expected latency and RTT
|
||||
EXPECT_EQ(offset, client.GetMetadata().offset);
|
||||
EXPECT_EQ(network_latency * 2, client.GetMetadata().rtt2);
|
||||
EXPECT_EQ(1u, client.GetMetadata().pongsReceived);
|
||||
EXPECT_EQ(pong_client_time, client.GetMetadata().lastPongTime);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user