PUBLIC

RegisC::RegisC(void) RegisC::RegisC(const RegisC &) RegisC::operator=(const RegisC &) RegisC::~RegisC(void) RegisC::RegisC(SArray1dC,SArray1dC) RegisC::RegisC(DListC,DListC) RegisC::RegisC(VectorSetC &,VectorSetC &) RegisC::RegisC(VectorSetC &,VectorSetC &,RealSArray1dC &,BooleanT,SceModT) RegisC::RegisC(Vector3dC,Vector3dC,Matrix3d3C) RegisC::CovTests(VectorSetC &,VectorSetC &,RealSArray1dC &,SceModT) RegisC::CovMat(VectorSetC &,RealSArray1dC &,RigidTransC,SceModT) RegisC::DevMat(VectorSetC &,VectorSetC &,RealSArray1dC &,RigidTransC,SceModT) RegisC::Cal_wi(double,SceModT) RegisC::Weight(VectorSetC &,VectorSetC &,RealSArray1dC &,SceModT) RegisC::Unique(void) RegisC::Reflect(void) RegisC::Transform(Vector3dC) RegisC::InvTransform(Vector3dC) RegisC::ExportTranslate(void) RegisC::ExportRotMat(void) RegisC::ExportRT(void) RegisC::ExportCovMat(void) RegisC::ExportSigma2(void) RegisC::LongPrint(void) RegisC::Print(BooleanT) RegisC::Cal_dr(Matrix3d3C &,Matrix3d3C &,Matrix3d3C &,const QuarternC &) RegisC::Cal_dr(Matrix3d3C &,Matrix3d3C &,Matrix3d3C &,const RigidTransC &) RegisC::Cal_mi(const Vector3dC &,const Matrix3d3C &,const Matrix3d3C &,const Matrix3d3C &) RegisC::JacobianDfeDe(const RigidTransC &,const RigidTransC &) RegisC::JacobianDr2_r1Dr1(const QuarternC &,const QuarternC &) RegisC::TestJacobians(void) RegisC::func_f(double,double) RegisC::func_g(double,double) RegisC::func_df(double,double) RegisC::func_dg(double,double) RegisC::func_tau(double,double) RegisC::func_ups(double,double)

RegisC
Registers two 3d data sets.
include	"amma/Regis.hh"
User Level:	Default
Library:	Regis
Example:	testcom.cc
Section:	3D Surface.Registration
In Scope:	std

Comments:

VERY QUICK EXPLANATION:

RegisC reg(base, query);
RigidTransC rt=reg.ExportRT(void);
rt will now contain the transform required to move base to register with query

MORE DETAILS:

This is a simple routine to register two 3d data sets. It assumes

that query[i] = rot * base[i] + trans + noise

and finds the Least Squares estimate of rot and trans. It is based on Arun, Huang & Blostein PAMI 9(5) 698-700 (1987) there are later enhancements to the method in Umeyama, PAMI 13(4) 376-380 (1991) and Kanatani, PAMI 16(5) 543-549 (1994). The result is always a proper rotation.

Member function Transform does rot * v + trans and InvTransform does rot^T * (v - trans)

Notes :

I use Vector3dC and Matrix3d3C throughout except where I need the SVD, where I use VectorC and MatrixC.

This routine always produces a result, which may however not be unique. The member function unique can be asked to provide the status. Examples of non-unique transformation are only 2 points or all points collinear.

Whether or not a tranf is unique depends on a threshold - nearlyzero in Regis.cc This threshold controls several things and should be developed a little further.

Programmers note:

Beware of errors caused by FixSmallToBeZero, use only for print out!

There is a switch in Regis.cc that can be used for some debugging

Some services additional to computing the pose and its covariance are available in particular Cal_dr is a standalone routine

Covariance Notes
----------------

For a meaningful covariance meaningful weights must be input, and the SceModT must be specified.

Point covariances are sigma2 = 3 rho2 - sigma2 is the rms Euclidean distance
- rho2 is the mean squared delta x, y or z

The weight[i] should be 1/sigma2. This is converted to wi = (2 for SceSce) diag[ 1/rho2, 1/rho2, 1/rho2]

Simplified uses include just setting the weights to 0/1 and not computing the covariance.

Note the details in the computation of sigma2.

const double threshold;

Used to select large or small angle computations See Cal_dr, JacobianDr2_r1Dr1.

const double nearlyzero;

This is used in two places: To test for singular value near zero and to test for weights

const double SVDisSmall;

a parameter for the amma SVD routine

const double threshold = 0.02;

Used in Cal_dr to choose one of 2 different methods to compute dR/dr

const double nearlyzero = 1e-10;

This is used in two places: To test for singular value near zero and to test for weights

const double SVDisSmall = 1e-14;

a parameter for the amma SVD routine

Matrix3d3C rot;

The result is computed using the SVD and stored in rot It is then converted to a RigidTransC and stored in tr The rot will possibly be just a minute fraction more accurate and is therefore retained and available for export, although most operations will use the RigidTransC

BooleanT unique;

To our best guess, is the result unique

BooleanT reflect;

Was the best solution actually a reflection, TRUE OR FALSE The returned answer is at present always the reflection.

RigidTransC tr;

BooleanT cov_cal;

MatrixC cov_tr;

double sigma2;

BooleanT done_sig;

RegisC()

Null Constructor

RegisC(const RegisC & v)

Copy constructor

RegisC & operator=(const RegisC & v)

~RegisC()

Destructor Constructors ------------ This routine is always called by supplying the data to a coinstructor

RegisC(SArray1dC<Vector3dC> base,SArray1dC<Vector3dC> query)

Constructor from point sets, passes to the 5 argument constructor

RegisC(DListC<Vector3dC> base,DListC<Vector3dC> query)

Constructor from point sets, passes to the 5 argument constructor

RegisC(VectorSetC & base,VectorSetC & query)

Constructor from point sets, passes to the 5 argument constructor

RegisC(VectorSetC & base,VectorSetC & query,RealSArray1dC & w,BooleanT do_cov = FALSE,SceModT reg_type = model_scene)

Constructor from point sets and weights The default should be set to model_scene so that ExportSigma2 makes sense to users not bothered with the difference between point covariance and the mean square residual distance.

RegisC(Vector3dC bcent,Vector3dC qcent,Matrix3d3C hh)

Constructor from centroids and H matrix, may be used directly but the Covariance and sigma2 will not be available.

void CovTests(VectorSetC & base,VectorSetC & query,RealSArray1dC & weights,SceModT reg_type)

Some internal consistency checks on the covariance routines

MatrixC CovMat(VectorSetC & base,RealSArray1dC & weights,RigidTransC tr0,SceModT reg_type)

Covariance matrix of the transformation, assumes rigid trans already computed.

VectorC DevMat(VectorSetC & base,VectorSetC & query,RealSArray1dC & weights,RigidTransC tr0,SceModT reg_type)

Only used for testing at the moment

double Cal_wi(double weight,SceModT reg_type)

Covariance matrix of error vector

void Weight(VectorSetC & base,VectorSetC & query,RealSArray1dC & weights,SceModT reg_type)

estimation of sigma2. See comments for ExportSigma2

BooleanT Unique()

returns true if the transform is unique

BooleanT Reflect()

returns true if the best transform was actually a reflection

Vector3dC Transform(Vector3dC v)

returns rot * v + trans

Vector3dC InvTransform(Vector3dC v)

returns rot^T * (v - trans)

Vector3dC ExportTranslate(void)

returns trans

Matrix3d3C ExportRotMat(void)

returns (a copy of) the rotation matrix

RigidTransC ExportRT(void)

returns the transformation

MatrixC ExportCovMat(void)

returns (a copy of) the covariance matrix of the transformation

double ExportSigma2(void)

returns sigma**2 the mean square point measurement error. This is based on actual point distances. Therefore it is not available for the centroid constructor. It depends on the selection of SceModT, and uses the default selected in the constructor if not supplied. For model_scene it is equivalent to the mean squared residual, otherwise it is half that. points with weight[i]

void LongPrint(void)

print out fullest details

void Print(BooleanT prtrans = TRUE)

print out details including the transfrom if required

void Cal_dr(Matrix3d3C & drx,Matrix3d3C & dry,Matrix3d3C & drz,const QuarternC & q)

void Cal_dr(Matrix3d3C & drx,Matrix3d3C & dry,Matrix3d3C & drz,const RigidTransC & rigid_tr)

MatrixC Cal_mi(const Vector3dC & base,const Matrix3d3C & drx,const Matrix3d3C & dry,const Matrix3d3C & drz)

d(Ra) The rotation R is the same as r=n theta, R=3x3, r=3x1. ----- dr Cal_dr returns dR_ij/dr_x dR_ij/dr_y dR_ij/dr_z If you want M = - d(f*a)/df = [ dR_ij/dr_x a_j | dR_ij/dr_y a_j | dR_ij/dr_z a_j | -1(3x3) ] use reg.Cal_dr( drx, dry, drz, f); MatrixC M = reg.Cal_mi(a, drx, dry, drz);

MatrixC JacobianDfeDe(const RigidTransC & f,const RigidTransC & e)

Computation of the Jacobian Je of the transformations composition f * e with respect to the transformation e.
This Jacobian is needed to propagate the covariance matrix We of a compositive noise e through the (noise-free) transformation f. The procedure comes from Pennec97a (Pennec, Thirion: A Framework for Uncertainty and Validation ..., Int. J. Comp. Vision, 25(3), 1997). Assuming the transformation f is noise-free, the resulting covariance: Wfe = Je We Je^T (from Pennec eq.5).

MatrixC JacobianDr2_r1Dr1(const QuarternC & q2,const QuarternC & q1)

Computation of the partial derivative D = dr/dr1, where r1 (q1) and r2 (q2) are rotations and compound rotation r = r2*r1.
We make use of the chain rule (see Pennec, for this and also for the notation and names of other variables, which we try to follow): D = dr/dre = dr/dq * dq/dq1 * dq1/dr1 = (notation) drdq * dqdq1 * dq1dr1

void TestJacobians()

Tests JacobianDr2_r1Dr1 numerically

double func_f(double t,double crossover)

= sin(t)/t

double func_g(double t,double crossover)

double func_df(double t,double crossover)

double func_dg(double t,double crossover)

double func_tau(double t,double crossover)

double func_ups(double t,double crossover)

the Taylor series of this have residual error of O(t^17) some intermediate trig functions needed for Jacobians