Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Added matrix determinant

Browse Source
This commit is contained in:
Ryan Manley
2022-01-31 19:39:33 -07:00
parent 9cc41f53af
commit 13e3b1e056
2 changed files with 49 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java
+34
View File
@@ -3,11 +3,20 @@ package frc4388.robot.commands.drive;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import org.ejml.simple.SimpleMatrix;
import org.opencv.core.Mat;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N5;
import edu.wpi.first.wpiutil.math.numbers.N6;
import frc4388.robot.Constants.VOPConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
@@ -74,7 +83,32 @@ public class VisionUpdateOdometry extends CommandBase {
// http://www.lee-mac.com/5pointellipse.html
// https://math.stackexchange.com/questions/163920/how-to-find-an-ellipse-given-five-points
// https://towardsdatascience.com/understanding-singular-value-decomposition-and-its-application-in-data-science-388a54be95d
// https://www.desmos.com/calculator/ounqguxjac
// https://en.wikipedia.org/wiki/Five_points_determine_a_conic
/* solves the following matrix
* | x0^2 x0y0 y0^2 x0 y0 1 |
* | x1^2 x1y1 y1^2 x1 y1 1 |
* det| x2^2 x2y2 y2^2 x2 y2 1 | = 0
* | x3^2 x3y3 y3^2 x3 y3 1 |
* | x4^2 x4y4 y4^2 x4 y4 1 |
* | x5^2 x5y5 y5^2 x5 y5 1 |
* for conic equation
* ax^2 - bxy + cy^2 - dx + fy - g = 0
*/
public static double[] getEllipseRadii(double[] xPoints, double[] yPoints) {
double[][] matrix = new double[5][6];
// Generate matrix
for(int i = 0; i < 5; i++) {
matrix[i][0] = xPoints[i] * xPoints[i];
matrix[i][1] = xPoints[i] * yPoints[i];
matrix[i][2] = yPoints[i] * yPoints[i];
matrix[i][3] = xPoints[i] * 1.d;
matrix[i][4] = 1.d * yPoints[i];
matrix[i][5] = 1.d;
}
return null;
}