Public Types | Public Member Functions | Static Public Member Functions | Static Public Attributes | List of all members
grl::PivotCalibrationVrepPlugin Class Reference

#include <PivotCalibrationVrepPlugin.hpp>

+ Inheritance diagram for grl::PivotCalibrationVrepPlugin:
+ Collaboration diagram for grl::PivotCalibrationVrepPlugin:

Public Types

enum  ParamIndex { ToolTipToModifyName, ToolTipToMeasureName, ToolBaseToModifyName, ToolBaseToMeasureName }
 
typedef std::tuple< std::string, std::string, std::string, std::string > Params
 

Public Member Functions

void addFrame ()
 
void applyEstimate ()
 Will apply the stored estimate to the v-rep simulation value. More...
 
void construct ()
 
void estimatePivotOffset ()
 run solver to estimate the unknown transform and set the simulation to the estimated value More...
 
Eigen::Affine3d getEstimate ()
 
 PivotCalibrationVrepPlugin (Params params=defaultParams())
 
void restoreSensorPosition ()
 Correct modified transform for measured optical tracker data. More...
 
void setAlgorithm (std::string algorithm_)
 

Static Public Member Functions

static const Params defaultParams ()
 

Static Public Attributes

static const bool debug = false
 

Detailed Description

Creates a complete vrep plugin object usage:

auto pivotCalib = std::make_shared<grl::PivotCalibrationVrepPlugin>();
pivotCalib->construct();
// move objects to new position
Todo:
the rotation component may be different between the

Definition at line 45 of file PivotCalibrationVrepPlugin.hpp.

Member Typedef Documentation

§ Params

typedef std::tuple< std::string, std::string, std::string, std::string > grl::PivotCalibrationVrepPlugin::Params

Definition at line 62 of file PivotCalibrationVrepPlugin.hpp.

Member Enumeration Documentation

§ ParamIndex

Enumerator
ToolTipToModifyName 

V-REP handle string for the tip of the tool that will be modified

ToolTipToMeasureName 

V-REP handle string for the tip of the tool that will be measured

ToolBaseToModifyName 

V-REP handle string for the base of the tool that will be modified

ToolBaseToMeasureName 

V-REP handle string for the base of the tool that will be measured

Definition at line 50 of file PivotCalibrationVrepPlugin.hpp.

Constructor & Destructor Documentation

§ PivotCalibrationVrepPlugin()

grl::PivotCalibrationVrepPlugin::PivotCalibrationVrepPlugin ( Params  params = defaultParams())
inline
Todo:
allow KukaFRIThreadSeparator parameters to be updated
Todo:
figure out how to re-enable when .so isn't loaded

Definition at line 75 of file PivotCalibrationVrepPlugin.hpp.

Member Function Documentation

§ addFrame()

void grl::PivotCalibrationVrepPlugin::addFrame ( )
inline

Definition at line 106 of file PivotCalibrationVrepPlugin.hpp.

§ applyEstimate()

void grl::PivotCalibrationVrepPlugin::applyEstimate ( )
inline

Will apply the stored estimate to the v-rep simulation value.

A default transform is saved when construct() was called so if no estimate has been found that will be used.

Definition at line 185 of file PivotCalibrationVrepPlugin.hpp.

§ construct()

void grl::PivotCalibrationVrepPlugin::construct ( )
inline

construct() function completes initialization of the plugin

Todo:
move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests.

Definition at line 88 of file PivotCalibrationVrepPlugin.hpp.

§ defaultParams()

static const Params grl::PivotCalibrationVrepPlugin::defaultParams ( )
inlinestatic

Definition at line 65 of file PivotCalibrationVrepPlugin.hpp.

§ estimatePivotOffset()

void grl::PivotCalibrationVrepPlugin::estimatePivotOffset ( )
inline

run solver to estimate the unknown transform and set the simulation to the estimated value

after calling addFrame a number of times in different positions and orientations

Todo:
evaluate if applyEstimate should not be called by this

TODO(ahundt) use Ransac pivot calibration wrapper because there will likely be noise

TODO(ahundt) Allow selection of "TWO_STEP_PROECDURE" and "COMBINATORIAL_APPROACH" from V-REP LUA API

Definition at line 140 of file PivotCalibrationVrepPlugin.hpp.

§ getEstimate()

Eigen::Affine3d grl::PivotCalibrationVrepPlugin::getEstimate ( )
inline

Definition at line 191 of file PivotCalibrationVrepPlugin.hpp.

§ restoreSensorPosition()

void grl::PivotCalibrationVrepPlugin::restoreSensorPosition ( )
inline

Correct modified transform for measured optical tracker data.

If real time transform measurements are happening applyEstimate will move the measured object relative to the sensor and this puts the transform measurement back so the object appears in its real position. However, if running in simulation only the sensor will be in the correct position, so you don't want to call this.

Definition at line 202 of file PivotCalibrationVrepPlugin.hpp.

§ setAlgorithm()

void grl::PivotCalibrationVrepPlugin::setAlgorithm ( std::string  algorithm_)
inline

Possible algorithms are: TWO_STEP_PROCEDURE, //!< First, estimate the pivot point in global coordinates. Second, estimate the sought translation by computing the difference vector between the pivot point and the center of the sensor system; then take the mean.
This algorithm is advantageous in the case of zero-mean noise. COMBINATORICAL_APPROACH //!< Setup n equations that yield the pivot point from a single measurement using the sought (but unknown) translation. Eliminate the pivot point in the system of equations and finally solve for the sought translation.
This algorithm is advantageous in the case of non-zero-mean noise (e.g. a systematic error).

If the algorithm string doesn't match an existing algorithm, this API will assume TWO_STEP_PROCEDURE.

Definition at line 100 of file PivotCalibrationVrepPlugin.hpp.

Member Data Documentation

§ debug

const bool grl::PivotCalibrationVrepPlugin::debug = false
static

Definition at line 48 of file PivotCalibrationVrepPlugin.hpp.


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