A class that deals with the problem of determining the roto-translation matrix H and scaling factors S between two sets of matching 3D points employing IpOpt. More...

#include <calibReference.h>

Inheritance diagram for iCub::optimization::CalibReferenceWithMatchedPoints:

Public Member Functions
	CalibReferenceWithMatchedPoints ()
	Default Constructor.

virtual void	setBounds (const yarp::sig::Matrix &min, const yarp::sig::Matrix &max)
	Allow specifying the minimum and maximum bounds of the elements belonging to the transformation.

virtual void	setBounds (const yarp::sig::Vector &min, const yarp::sig::Vector &max)
	Allow specifying the bounding box for both the translation part (given in meters) and the rotation part (given in radians) within which solution is seeked.

virtual void	setScalingBounds (const yarp::sig::Vector &min, const yarp::sig::Vector &max)
	Allow specifying the bounds for the 3D scaling factors.

virtual void	setScalingBounds (const double min, const double max)
	Allow specifying the bounds for the 3D scaling factors with scalar scaling factor.

virtual bool	addPoints (const yarp::sig::Vector &p0, const yarp::sig::Vector &p1)
	Add to the internal database the 3D-point p0 and the 3D-point p1 which is supposed to correspond to H*p0, whose matrix H has to be found.

virtual size_t	getNumPoints () const
	Return the number of 3D-points pairs currently contained into the internal database.

virtual void	getPoints (std::deque< yarp::sig::Vector > &p0, std::deque< yarp::sig::Vector > &p1) const
	Retrieve copies of the database of 3D-points pairs.

virtual void	clearPoints ()
	Clear the internal database of 3D points.

virtual bool	setInitialGuess (const yarp::sig::Matrix &H)
	Allow specifiying the initial guess for the roto-translation matrix we seek for.

virtual bool	setScalingInitialGuess (const yarp::sig::Vector &s)
	Allow specifiying the initial guess for the scaling factors.

virtual bool	setScalingInitialGuess (const double s)
	Allow specifiying the initial guess for the scalar scaling factor.

virtual bool	setCalibrationOptions (const yarp::os::Property &options)
	Allow setting further options used during calibration.

virtual bool	calibrate (yarp::sig::Matrix &H, double &error)
	Perform reference calibration to determine the matrix H.

virtual bool	calibrate (yarp::sig::Matrix &H, yarp::sig::Vector &s, double &error)
	Perform reference calibration to determine the matrix H and the scaling factors S.

virtual bool	calibrate (yarp::sig::Matrix &H, double &s, double &error)
	Perform reference calibration to determine the matrix H and the scalar scaling factor s.

virtual	~CalibReferenceWithMatchedPoints ()
	Destructor.

Public Member Functions inherited from iCub::optimization::MatrixTransformationWithMatchedPoints
virtual	~MatrixTransformationWithMatchedPoints ()
	Destructor.

Protected Member Functions
double	evalError (const yarp::sig::Matrix &H)

Protected Attributes
yarp::sig::Vector	min

yarp::sig::Vector	min_s

yarp::sig::Vector	max

yarp::sig::Vector	max_s

yarp::sig::Vector	x0

yarp::sig::Vector	s0

int	max_iter

double	tol

double	min_s_scalar

double	max_s_scalar

double	s0_scalar

std::deque< yarp::sig::Vector >	p0

std::deque< yarp::sig::Vector >	p1

Detailed Description

A class that deals with the problem of determining the roto-translation matrix H and scaling factors S between two sets of matching 3D points employing IpOpt.

The problem solved is of the form:

\[ (H,S)=\arg\min_{H\in SE\left(3\right),S\in diag\left(s_1,s_2,s_3,1\right)}\left(\frac{1}{N}\sum_{i=1}^{N} \left \| p_i^{O_1}-S \cdot H \cdot p_i^{O_2} \right \|^2 \right) \]

Definition at line 54 of file calibReference.h.

Constructor & Destructor Documentation

◆ CalibReferenceWithMatchedPoints()

CalibReferenceWithMatchedPoints::CalibReferenceWithMatchedPoints ( )

Default Constructor.

Definition at line 467 of file calibReference.cpp.

◆ ~CalibReferenceWithMatchedPoints()

virtual iCub::optimization::CalibReferenceWithMatchedPoints::~CalibReferenceWithMatchedPoints ( )

inlinevirtual

Destructor.

Definition at line 235 of file calibReference.h.

Member Function Documentation

◆ addPoints()

bool CalibReferenceWithMatchedPoints::addPoints	(	const yarp::sig::Vector &	p0,
		const yarp::sig::Vector &	p1
	)

virtual

Add to the internal database the 3D-point p0 and the 3D-point p1 which is supposed to correspond to H*p0, whose matrix H has to be found.

Parameters

p0	the free 3D-point.
p1	the 3D-point which corresponds either to Hp0 or to SH*p0.

Returns: true/false on success/fail.

Note: points are stored in homogeneous convention (i.e. 4x1 with trailing 1).

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

Definition at line 567 of file calibReference.cpp.

◆ calibrate() [1/3]

virtual bool iCub::optimization::CalibReferenceWithMatchedPoints::calibrate	(	yarp::sig::Matrix &	H,
		double &	error
	)

virtual

Perform reference calibration to determine the matrix H.

Parameters

H	the final roto-translation matrix that links the two reference frames of 3D points.
error	returns the residual error computed as norm(p1[i]-H*p0[i]) over the whole set of points pairs.

Returns: true/false on success/fail.

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

◆ calibrate() [2/3]

virtual bool iCub::optimization::CalibReferenceWithMatchedPoints::calibrate	(	yarp::sig::Matrix &	H,
		double &	s,
		double &	error
	)

virtual

Perform reference calibration to determine the matrix H and the scalar scaling factor s.

Parameters

H	the final roto-translation matrix that links the two reference frames of 3D points.
s	the found scaling factor.
error	returns the residual error computed as norm(p1[i]-sHp0[i]) over the whole set of points pairs.

Returns: true/false on success/fail.

◆ calibrate() [3/3]

virtual bool iCub::optimization::CalibReferenceWithMatchedPoints::calibrate	(	yarp::sig::Matrix &	H,
		yarp::sig::Vector &	s,
		double &	error
	)

virtual

Perform reference calibration to determine the matrix H and the scaling factors S.

Parameters

H	the final roto-translation matrix that links the two reference frames of 3D points.
s	the 3x1 vector containing the found scaling factors.
error	returns the residual error computed as norm(p1[i]-SHp0[i]) over the whole set of points pairs, where S is the diagonal 3x3 matrix containing the scaling factors.

Returns: true/false on success/fail.

◆ clearPoints()

void CalibReferenceWithMatchedPoints::clearPoints ( )

virtual

Clear the internal database of 3D points.

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

Definition at line 595 of file calibReference.cpp.

◆ evalError()

double CalibReferenceWithMatchedPoints::evalError ( const yarp::sig::Matrix & H )

protected

Definition at line 551 of file calibReference.cpp.

◆ getNumPoints()

virtual size_t iCub::optimization::CalibReferenceWithMatchedPoints::getNumPoints ( ) const

inlinevirtual

Return the number of 3D-points pairs currently contained into the internal database.

Returns: the number of pairs.

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

Definition at line 146 of file calibReference.h.

◆ getPoints()

void CalibReferenceWithMatchedPoints::getPoints	(	std::deque< yarp::sig::Vector > &	p0,
		std::deque< yarp::sig::Vector > &	p1
	)		const

virtual

Retrieve copies of the database of 3D-points pairs.

Parameters

p0	the list of free 3D-points.
p1	the list of 3D-points which correspond either to Hp0 or to SH*p0.

Note: points are retrived in 4x1 homogeneous format.

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

Definition at line 586 of file calibReference.cpp.

◆ setBounds() [1/2]

virtual void iCub::optimization::CalibReferenceWithMatchedPoints::setBounds	(	const yarp::sig::Matrix &	min,
		const yarp::sig::Matrix &	max
	)

virtual

Allow specifying the minimum and maximum bounds of the elements belonging to the transformation.

Parameters

min	the 4x4 Matrix containining the minimum bounds. The translation bounds are given in min(0:2,3), whereas the rotation bounds are given in min(0,0:2).
max	the 4x4 Matrix containining the maximum bounds. The translation bounds are given in max(0:2,3), whereas the rotation bounds are given in max(0,0:2).

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

◆ setBounds() [2/2]

virtual void iCub::optimization::CalibReferenceWithMatchedPoints::setBounds	(	const yarp::sig::Vector &	min,
		const yarp::sig::Vector &	max
	)

virtual

Allow specifying the bounding box for both the translation part (given in meters) and the rotation part (given in radians) within which solution is seeked.

Parameters

min	the 6x1 Vector containining the minimum bounds. The first 3 dimensions account for the translation part, the last 3 for the rotation in Euler angles representation.
max	the 6x1 Vector containining the maximum bounds. The first 3 dimensions account for the translation part, the last 3 for the rotation in Euler angles representation.

Note: by default min=(-1.0,-1.0,-1.0,-M_PI,-M_PI,-M_PI) and max=(1.0,1.0,1.0,M_PI,M_PI,M_PI).

◆ setCalibrationOptions()

bool CalibReferenceWithMatchedPoints::setCalibrationOptions ( const yarp::os::Property & options )

virtual

Allow setting further options used during calibration.

Parameters

options a Property-like object accounting for calibration options.

Returns: true/false on success/fail.

Reimplemented from iCub::optimization::MatrixTransformationWithMatchedPoints.

Definition at line 640 of file calibReference.cpp.

◆ setInitialGuess()

bool CalibReferenceWithMatchedPoints::setInitialGuess ( const yarp::sig::Matrix & H )

virtual

Allow specifiying the initial guess for the roto-translation matrix we seek for.

Parameters

H	the 4x4 homogeneous matrix used as initial guess.

Returns: true/false on success/fail.

Implements iCub::optimization::MatrixTransformationWithMatchedPoints.

Definition at line 603 of file calibReference.cpp.

◆ setScalingBounds() [1/2]

void CalibReferenceWithMatchedPoints::setScalingBounds	(	const double	min,
		const double	max
	)

virtual

Allow specifying the bounds for the 3D scaling factors with scalar scaling factor.

Parameters

min	the the minimum bound.
max	the the maximum bound.

Note: by default min=0.1 and max=10.0.

Definition at line 542 of file calibReference.cpp.

◆ setScalingBounds() [2/2]

virtual void iCub::optimization::CalibReferenceWithMatchedPoints::setScalingBounds	(	const yarp::sig::Vector &	min,
		const yarp::sig::Vector &	max
	)

virtual

Allow specifying the bounds for the 3D scaling factors.

Parameters

min	the 3x1 Vector containining the minimum bounds.
max	the 3x1 Vector containining the maximum bounds.

Note: by default min=(0.1,0.1,0.1) and max=(10.0,10.0,10.0).

◆ setScalingInitialGuess() [1/2]

bool CalibReferenceWithMatchedPoints::setScalingInitialGuess ( const double s )

virtual

Allow specifiying the initial guess for the scalar scaling factor.

Parameters

s	the scaling factor.

Returns: true/false on success/fail.

Definition at line 632 of file calibReference.cpp.

◆ setScalingInitialGuess() [2/2]

virtual bool iCub::optimization::CalibReferenceWithMatchedPoints::setScalingInitialGuess ( const yarp::sig::Vector & s )

virtual

Allow specifiying the initial guess for the scaling factors.

Parameters

s	the 3x1 vector of scaling factors.

Returns: true/false on success/fail.

Member Data Documentation

◆ max

yarp::sig::Vector iCub::optimization::CalibReferenceWithMatchedPoints::max

protected

Definition at line 58 of file calibReference.h.

◆ max_iter

int iCub::optimization::CalibReferenceWithMatchedPoints::max_iter

protected

Definition at line 61 of file calibReference.h.

◆ max_s

yarp::sig::Vector iCub::optimization::CalibReferenceWithMatchedPoints::max_s

protected

Definition at line 58 of file calibReference.h.

◆ max_s_scalar

double iCub::optimization::CalibReferenceWithMatchedPoints::max_s_scalar

protected

Definition at line 64 of file calibReference.h.

◆ min

yarp::sig::Vector iCub::optimization::CalibReferenceWithMatchedPoints::min

protected

Definition at line 57 of file calibReference.h.

◆ min_s

yarp::sig::Vector iCub::optimization::CalibReferenceWithMatchedPoints::min_s

protected

Definition at line 57 of file calibReference.h.

◆ min_s_scalar

double iCub::optimization::CalibReferenceWithMatchedPoints::min_s_scalar

protected

Definition at line 63 of file calibReference.h.

◆ p0

std::deque<yarp::sig::Vector> iCub::optimization::CalibReferenceWithMatchedPoints::p0

protected

Definition at line 67 of file calibReference.h.

◆ p1

std::deque<yarp::sig::Vector> iCub::optimization::CalibReferenceWithMatchedPoints::p1

protected

Definition at line 68 of file calibReference.h.

◆ s0

yarp::sig::Vector iCub::optimization::CalibReferenceWithMatchedPoints::s0

protected

Definition at line 59 of file calibReference.h.

◆ s0_scalar

double iCub::optimization::CalibReferenceWithMatchedPoints::s0_scalar

protected

Definition at line 65 of file calibReference.h.

◆ tol

double iCub::optimization::CalibReferenceWithMatchedPoints::tol

protected

Definition at line 62 of file calibReference.h.

◆ x0

yarp::sig::Vector iCub::optimization::CalibReferenceWithMatchedPoints::x0

protected

Definition at line 59 of file calibReference.h.

The documentation for this class was generated from the following files:

icub-main/src/libraries/optimization/include/iCub/optimization/calibReference.h
icub-main/src/libraries/optimization/src/calibReference.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CalibReferenceWithMatchedPoints()

◆ ~CalibReferenceWithMatchedPoints()

Member Function Documentation

◆ addPoints()

◆ calibrate() [1/3]

◆ calibrate() [2/3]

◆ calibrate() [3/3]

◆ clearPoints()

◆ evalError()

◆ getNumPoints()

◆ getPoints()

◆ setBounds() [1/2]

◆ setBounds() [2/2]

◆ setCalibrationOptions()

◆ setInitialGuess()

◆ setScalingBounds() [1/2]

◆ setScalingBounds() [2/2]

◆ setScalingInitialGuess() [1/2]

◆ setScalingInitialGuess() [2/2]

Member Data Documentation

◆ max

◆ max_iter

◆ max_s

◆ max_s_scalar

◆ min

◆ min_s

◆ min_s_scalar

◆ p0

◆ p1

◆ s0

◆ s0_scalar

◆ tol

◆ x0