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 quiet_delete_file(const std::string & filename)
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

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

function readSparseShape

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

function writeSparseShape

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

function readSparseShape

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

function writeSparseShape

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

function readParticleIds

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

function writeParticleIds

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

function quiet_delete_file

cpp static void quiet_delete_file( const std::string & filename )

function computeCenterOfMassForShapeEnsemble

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

function computeCenterOfMassForShape

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

function updateMin

cpp static void updateMin( double curVal, double & minVal )

function updateMax

cpp static void updateMax( double curVal, double & maxVal )

function getBoundingBoxForShapeEnsemble

cpp 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

cpp 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

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

function cartesian2spherical

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

function convertToPhysicalCoordinates

cpp 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

cpp 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

cpp static std::string num2str( float num )

function num2str

cpp static std::string num2str( int num )

function linspace

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

function int2str

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

function multiply_into

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

function averageThetaBruteForce

cpp 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][]'

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

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

function averageThetaArc

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

Updated on 2026-03-31 at 16:02:10 +0000