Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Noise qualification update #65

Merged
merged 1 commit into from
Jun 29, 2020
Merged

Noise qualification update #65

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 8 additions & 13 deletions rct_optimizations/src/rct_optimizations/validation/noise_qualification.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <rct_optimizations/validation/noise_qualification.h>
#include <rct_optimizations/pnp.h>

Expand All @@ -20,8 +20,8 @@ Eigen::Quaterniond computeQuaternionMean(const std::vector<Eigen::Quaterniond>&
* M = sum(w_i * q_i * q_i^T) Eq. 12
* q_bar = argmax(q^T * M * q) Eq. 13
*
* The solution of this maximization problem is well known. The average quaternion is
* the eigenvector of M corresponding to the maximum eigenvalue.
* "The solution of this maximization problem is well known. The average quaternion is
* the eigenvector of M corresponding to the maximum eigenvalue."
Comment on lines +23 to +24
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are these "editorial" kinds of quotes?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That was a quote that I pulled directly from the paper. I probably should do a better job citing it, but I at least wanted to indicate that that particular comment was not my own

*
* In the above equations, w_i is the weight of the ith quaternion.
* In this case, all quaternions are equally weighted (i.e. w_i = 1)
Expand All @@ -34,17 +34,12 @@ Eigen::Quaterniond computeQuaternionMean(const std::vector<Eigen::Quaterniond>&
M += q.coeffs() * q.coeffs().transpose();
}

// Eigenvectors,values should be strictly real
Eigen::EigenSolver<Eigen::Matrix4d> E(M, true);

// Each column of 4x4 vectors is an eigenvector; desired mean has max
Eigen::Index idx_max_ev; // Index of the largest eigenvalue

//find maximium eigenvalue, and store its index in max_evi
E.eigenvalues().real().maxCoeff(&idx_max_ev);
// Calculate the SVD of the M matrix
Eigen::JacobiSVD<Eigen::Matrix4d> svd(M, Eigen::ComputeFullU);

// The eigenvectors are represented by the columns of the U matrix; the eigenvector corresponding to the largest eigenvalue is in row 0
Eigen::Quaterniond q;
q.coeffs() << E.eigenvectors().real().col(idx_max_ev);
q.coeffs() << svd.matrixU().col(0);

assert(std::isnan(q.w()) == false &&
std::isnan(q.x()) == false &&
Expand All @@ -64,7 +59,7 @@ QuaternionStats computeQuaternionStats(const std::vector<Eigen::Quaterniond> &qu
{
q_var += std::pow(q_stats.mean.angularDistance(q), 2.0);
}
q_var /= static_cast<double>(quaternions.size());
q_var /= static_cast<double>(quaternions.size() - 1);
q_stats.stdev = std::sqrt(q_var);

return q_stats;
Expand Down