brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.9.6 release

Browse Source 
date: 2024-02-21T23:02:42
master commit: 0b4d08fab8e35a264bc7383e878538f8083c33e5
This commit is contained in:
FrogAi
2024-02-27 16:34:45 -07:00
commit 2901597132
1940 changed files with 647891 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
rednose/templates/compute_pos.c Normal file
View File

@@ -0,0 +1,52 @@
#include <eigen3/Eigen/QR>
#include <eigen3/Eigen/Dense>
#include <iostream>
typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
typedef Eigen::Matrix<double, 3, 1> O1M;
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
double res[KDIM*2] = {0};
double jac[KDIM*6] = {0};
O1M x(in_x);
O1M delta;
int counter = 0;
while ((delta.squaredNorm() > 0.0001 and counter < 30) or counter == 0){
res_fun(in_x, in_poses, in_img_positions, res);
jac_fun(in_x, in_poses, in_img_positions, jac);
R1M E(res); R3M J(jac);
delta = (J.transpose()*J).inverse() * J.transpose() * E;
x = x - delta;
memcpy(in_x, x.data(), 3 * sizeof(double));
counter = counter + 1;
}
}
void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) {
param[0] = img_positions[KDIM*2-2];
param[1] = img_positions[KDIM*2-1];
param[2] = 0.1;
gauss_newton(param, poses, img_positions);
Eigen::Quaterniond q;
q.w() = poses[KDIM*7-4];
q.x() = poses[KDIM*7-3];
q.y() = poses[KDIM*7-2];
q.z() = poses[KDIM*7-1];
M3D RC(to_c);
Eigen::Matrix3d R = q.normalized().toRotationMatrix();
Eigen::Matrix3d rot = R * RC.transpose();
pos[0] = param[0]/param[2];
pos[1] = param[1]/param[2];
pos[2] = 1.0/param[2];
O1M ecef_offset(poses + KDIM*7-7);
O1M ecef_output(pos);
ecef_output = rot*ecef_output + ecef_offset;
memcpy(pos, ecef_output.data(), 3 * sizeof(double));
}