Skip to content

Utils

Public Functions

Name
std::vector< int > randperm(int n)
void readSparseShape(vtkSmartPointer< vtkPoints > & points, char * filename, int number_of_particles =-1)
void writeSparseShape(char * filename, vtkSmartPointer< vtkPoints > particles)
void readSparseShape(std::vector< itk::Point< double > > & points, char * filename, int number_of_particles =-1)
void writeSparseShape(char * filename, std::vector< itk::Point< double, 3 > > points)
std::vector< int > readParticleIds(char * filename)
void writeParticleIds(char * filename, std::vector< int > ids)
void computeCenterOfMassForShapeEnsemble(std::vector< std::vector< itk::Point< double, 3 > > > points_list, itk::Point< double, 3 > & center)
void computeCenterOfMassForShape(std::vector< itk::Point< double, 3 > > points, itk::Point< double, 3 > & center)
void updateMin(double curVal, double & minVal)
void updateMax(double curVal, double & maxVal)
void getBoundingBoxForShapeEnsemble(std::vector< std::vector< itk::Point< double, 3 > > > points_list, double & min_x, double & min_y, double & min_z, double & max_x, double & max_y, double & max_z)
void getBoundingBoxForShape(std::vector< itk::Point< double, 3 > > points, double & min_x, double & min_y, double & min_z, double & max_x, double & max_y, double & max_z)
void spherical2cartesian(const double inPoint[3], double outPoint[3])
void cartesian2spherical(const double inPoint[3], double outPoint[3])
vtkSmartPointer< vtkPoints > convertToPhysicalCoordinates(vtkSmartPointer< vtkPoints > particles, int number_of_particles, const itk::Image< float, 3 >::SpacingType & spacing, const itk::Image< float, 3 >::PointType & origin)
vtkSmartPointer< vtkPoints > convertToImageCoordinates(vtkSmartPointer< vtkPoints > particles, int number_of_particles, const itk::Image< float, 3 >::SpacingType & spacing, const itk::Image< float, 3 >::PointType & origin)
std::string num2str(float num)
std::string num2str(int num)
std::vector< double > linspace(double a, double b, size_t N)
std::string int2str(int n, int number_of_zeros)
template <typename T >
void		 multiply_into(Eigen::MatrixXd & out, const vnl_matrix< T > & lhs, const vnl_matrix< T > & rhs)
double averageThetaBruteForce(std::vector< double > thetas, double dtheta)
double averageThetaChord(std::vector< double > thetas)
double averageThetaArc(std::vector< double > thetas)

Public Functions Documentation

function randperm

static std::vector< int > randperm(
    int n
)

function readSparseShape

static void readSparseShape(
    vtkSmartPointer< vtkPoints > & points,
    char * filename,
    int number_of_particles =-1
)

function writeSparseShape

static void writeSparseShape(
    char * filename,
    vtkSmartPointer< vtkPoints > particles
)

function readSparseShape

static void readSparseShape(
    std::vector< itk::Point< double > > & points,
    char * filename,
    int number_of_particles =-1
)

function writeSparseShape

static void writeSparseShape(
    char * filename,
    std::vector< itk::Point< double, 3 > > points
)

function readParticleIds

static std::vector< int > readParticleIds(
    char * filename
)

function writeParticleIds

static void writeParticleIds(
    char * filename,
    std::vector< int > ids
)

function computeCenterOfMassForShapeEnsemble

static void computeCenterOfMassForShapeEnsemble(
    std::vector< std::vector< itk::Point< double, 3 > > > points_list,
    itk::Point< double, 3 > & center
)

function computeCenterOfMassForShape

static void computeCenterOfMassForShape(
    std::vector< itk::Point< double, 3 > > points,
    itk::Point< double, 3 > & center
)

function updateMin

static void updateMin(
    double curVal,
    double & minVal
)

function updateMax

static void updateMax(
    double curVal,
    double & maxVal
)

function getBoundingBoxForShapeEnsemble

static void getBoundingBoxForShapeEnsemble(
    std::vector< std::vector< itk::Point< double, 3 > > > points_list,
    double & min_x,
    double & min_y,
    double & min_z,
    double & max_x,
    double & max_y,
    double & max_z
)

function getBoundingBoxForShape

static void getBoundingBoxForShape(
    std::vector< itk::Point< double, 3 > > points,
    double & min_x,
    double & min_y,
    double & min_z,
    double & max_x,
    double & max_y,
    double & max_z
)

function spherical2cartesian

static void spherical2cartesian(
    const double inPoint[3],
    double outPoint[3]
)

function cartesian2spherical

static void cartesian2spherical(
    const double inPoint[3],
    double outPoint[3]
)

function convertToPhysicalCoordinates

static vtkSmartPointer< vtkPoints > convertToPhysicalCoordinates(
    vtkSmartPointer< vtkPoints > particles,
    int number_of_particles,
    const itk::Image< float, 3 >::SpacingType & spacing,
    const itk::Image< float, 3 >::PointType & origin
)

function convertToImageCoordinates

static vtkSmartPointer< vtkPoints > convertToImageCoordinates(
    vtkSmartPointer< vtkPoints > particles,
    int number_of_particles,
    const itk::Image< float, 3 >::SpacingType & spacing,
    const itk::Image< float, 3 >::PointType & origin
)

function num2str

static std::string num2str(
    float num
)

function num2str

static std::string num2str(
    int num
)

function linspace

static std::vector< double > linspace(
    double a,
    double b,
    size_t N
)

function int2str

static std::string int2str(
    int n,
    int number_of_zeros
)

function multiply_into

template <typename T >
static void multiply_into(
    Eigen::MatrixXd & out,
    const vnl_matrix< T > & lhs,
    const vnl_matrix< T > & rhs
)

function averageThetaBruteForce

static double averageThetaBruteForce(
    std::vector< double > thetas,
    double dtheta
)

Given a set of theta measurements, pick the "average" (approximately).

More formally, given a set of orientations, we wish to identify a "reference theta" such that the sum of the squared differences between each theta and the reference theta is minimized. This can be visualized: each theta (including the reference theta) can be mapped onto the unit circle): we wish to minimize the distance between the reference point and every other points by traveling along the circumference of the unit circle.

APPROXIMATE CHORD SOLUTION

This is hard, however, so instead of computing the distance along the circumference, we compute the distance along the chord.

This method is by ebolson@umich.edu, inspired by a similar problem in Horn's "closed-form solution of absolute orientation using unit quaternions".

Let a be the set of input points, and R(a_i) represent a rotation of point a_i around the origin:

R(x) = [ cos(theta)a_x - sin(theta)a_y,] [ sin(theta)a_x + cos(theta)a_y ]

The error is:

X^2 = SUM ( R(a_i) - [1 0]' )' * (R(a_i) - [1 0]')

= SUM R'R - 2[1 0]R(a) + [1 0][1 0]'

Note that R'R is constant, because R and R' are orthogonal. (R'R = I). Dropping constant terms:

X^2 = SUM 2[1 0]R(a)

Differentiating with respect to theta:

dX^2/dtheta = SUM cos(theta)a_x - sin(theta)a_y = 0

Collecting cos and sin terms:

cos(theta) SUM a_x = sin(theta) SUM a_y

e.g.,:

theta = atan2( SUM a_y , SUM a_x )

EXACT SOLUTION

This solution runs in O(n log n).

Let us suppose that all of the input angles are mapped to [-PI, PI].

All the input points can be shifted to be within PI degrees of the reference angle by adding a multiple of 2PI degrees. If all the input angles are constrained to [-PI, PI], then we can find a reference angle [-PI, 2PI] such that all input points are within PI degrees by either adding 0 or exactly 2PI to individual input points.

More so, the input points that we must add 2PI to are the M points with the smallest theta, but we do not know M. This is necessary when the correct reference angle is large: the smallest points will be more than PI degrees away, so they need to be moved to the right side of the reference angle.

If we knew M, computing the reference angle is easy: it is simply the average of the (possibly shifted) input points. Let x[i] be the input point [-PI,PI] and y[i] be the possibly shifted version of that point, y[i] = x[i] + 2PI if i < M, otherwise y[i] = x[i].

r = reference angle = (1 / N) * SUM_i y[i] error = SUM_i (y[i] - r)^2

We simply search over each value of M (from 0 to N), and recompute the error. Both the reference angle and error can be written in terms of the first and second moments of y[i], which gives us the following strategy:

1) Compute A1 and A2, the first and second moments of y[i], assuming M = 0. (This is just the first and second moments of x[i]). This involves iterating over each of the input points.

2) Considering the points in x[i] in sorted order, update A1 and A2 such that they reflect y[i] = x[i] + 2PI. Compute the new reference theta and error after every point (an O(1) operation) and report the theta whose error was the smallest.

Total run time is O(N log N) due to the sort operation. The other two passes are O(N). Memory usage is O(N), since all points must be stored so they can be sorted.

SUMMARY

method runtime memory notes

brute O(2PI*N / eps) O(N) worst-case error is eps/2 exact O(N log N) O(N) chord O(N) O(1) minimizes squared chord length, not squared arc length.

Real-world performance: the exact method is typically faster than the chord method, presumably because of the high cost of computing trigonometric functions used in the Chord method. This advantage decreases with larger number of points (due to the super-linear cost of sorting), but even at 50000 points, the optimal method is (a bit) faster than the chord method.

Reference: Olson, Edwin. "On computing the average orientation of vectors and lines." In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 3869-3874. IEEE, 2011.

Code is written in C++ from author's java implmentation by Shireen Elhabian - SCI institute, University of Utah

function averageThetaChord

static double averageThetaChord(
    std::vector< double > thetas
)

function averageThetaArc

static double averageThetaArc(
    std::vector< double > thetas
)

Updated on 2024-03-17 at 12:58:44 -0600