mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-22 01:11:40 +00:00
abbf3f2820
## Description instead of using camera calibration, the CasADi problem formulation now expect the caller to provide "normalized pixel coordinates". This reduces the number of floating point operations we need to perform (and reduces total LOC by 6%). `casadi_wrapper.cpp` now converts from (u, v) coordinates to normalized (x'', y'') coordinates with: x'' = (u - c_x) / f_x y'' = (u - c_y) / f_y Which is the inverse of this (from [opencv docs](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html), and assuming no distortion):  In my testing, this is ~16% faster on my x86 laptop. Would love some rio benchmarks. ## Meta Merge checklist: - [x] Pull Request title is [short, imperative summary](https://cbea.ms/git-commit/) of proposed changes - [x] The description documents the _what_ and _why_ - [~] If this PR changes behavior or adds a feature, user documentation is updated - [~] If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly - [~] If this PR touches configuration, this is backwards compatible with settings back to v2024.3.1 - [~] If this PR addresses a bug, a regression test for it is added
189 lines
5.5 KiB
C++
189 lines
5.5 KiB
C++
/*
|
|
* Copyright (C) Photon Vision.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include <chrono>
|
|
#include <cstdio>
|
|
#include <iostream>
|
|
#include <vector>
|
|
|
|
#include <wpi/timestamp.h>
|
|
|
|
#include "photon/constrained_solvepnp/wrap/casadi_wrapper.h"
|
|
|
|
#define TAG_COUNT 6
|
|
#if TAG_COUNT < 1
|
|
#error TAG_COUNT cannot be less than 1!
|
|
#endif
|
|
|
|
using casadi_real = double;
|
|
|
|
void print_cost(casadi_real robot_x, casadi_real robot_y,
|
|
casadi_real robot_theta) {
|
|
casadi_real fx = 600;
|
|
casadi_real fy = 600;
|
|
casadi_real cx = 300;
|
|
casadi_real cy = 150;
|
|
|
|
constexpr int NUM_LANDMARKS = 4 * TAG_COUNT;
|
|
|
|
// Note that casadi is column major, apparently
|
|
Eigen::Matrix<casadi_real, 4, 4, Eigen::ColMajor> robot2camera;
|
|
// clang-format off
|
|
robot2camera <<
|
|
0, 0, 1, 0,
|
|
-1, 0, 0, 0,
|
|
0, -1, 0, 0,
|
|
0, 0, 0, 1;
|
|
// clang-format on
|
|
|
|
Eigen::Matrix<casadi_real, NUM_LANDMARKS, 4, Eigen::ColMajor> field2points_;
|
|
// clang-format off
|
|
field2points_ <<
|
|
#if TAG_COUNT > 7
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
#if TAG_COUNT > 6
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
#if TAG_COUNT > 5
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
#if TAG_COUNT > 4
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
#if TAG_COUNT > 3
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
#if TAG_COUNT > 2
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
#if TAG_COUNT > 1
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1,
|
|
#endif
|
|
2.5, 0 - 0.08255, 0.5 - 0.08255, 1,
|
|
2.5, 0 - 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 + 0.08255, 1,
|
|
2.5, 0 + 0.08255, 0.5 - 0.08255, 1;
|
|
// clang-format on
|
|
Eigen::Matrix<casadi_real, 4, NUM_LANDMARKS, Eigen::ColMajor> field2points =
|
|
field2points_.transpose();
|
|
|
|
Eigen::Matrix<casadi_real, NUM_LANDMARKS, 2, Eigen::ColMajor>
|
|
point_observations_;
|
|
// clang-format off
|
|
point_observations_ <<
|
|
#if TAG_COUNT > 7
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
#if TAG_COUNT > 6
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
#if TAG_COUNT > 5
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
#if TAG_COUNT > 4
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
#if TAG_COUNT > 3
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
#if TAG_COUNT > 2
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
#if TAG_COUNT > 1
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17,
|
|
#endif
|
|
333, -17,
|
|
333, -83,
|
|
267, -83,
|
|
267, -17;
|
|
// clang-format on
|
|
Eigen::Matrix<casadi_real, 2, NUM_LANDMARKS, Eigen::ColMajor>
|
|
point_observations = point_observations_.transpose();
|
|
|
|
Eigen::Vector3d x_guess;
|
|
x_guess << robot_x, robot_y, robot_theta;
|
|
|
|
for (int i = 0; i < 200; i++) {
|
|
auto start = wpi::Now();
|
|
auto x_out = constrained_solvepnp::do_optimization(
|
|
true, TAG_COUNT,
|
|
constrained_solvepnp::CameraCalibration{fx, fy, cx, cy}, robot2camera,
|
|
x_guess, field2points, point_observations, 0, 0);
|
|
auto end = wpi::Now();
|
|
|
|
std::cout << i << "," << static_cast<bool>(x_out) << "," << end - start
|
|
<< std::endl;
|
|
std::cout << "Solution:"
|
|
<< x_out.value_or(constrained_solvepnp::RobotStateMat::Identity())
|
|
<< std::endl;
|
|
// std::cout << "iter "
|
|
// << i
|
|
// // << "\nGuess:\n" << x_guess << "\n Optimized ->\n"
|
|
// // << std::endl <<
|
|
// // x_out.value_or(constrained_solvepnp::RobotStateMat::Zero())
|
|
// << " - Succeeded? " << static_cast<bool>(x_out) << " - In "
|
|
// << (end - start) << "uS" << std::endl;
|
|
}
|
|
}
|
|
|
|
TEST(CasadiWrapperTest, smoketest) { print_cost(0.1, 0.1, 0.0); }
|