//--------------------------------------------------------------
void ArmCalibration::keyPressed(int key)
{
enum ArmCalibrationMode
{
kReadingClouds = 0,
kCheckingValues
};
static ArmCalibrationMode mode = kReadingClouds;
if (key == 'm')
{
mode = mode == kReadingClouds ? kCheckingValues : kReadingClouds;
}
if (mode == kReadingClouds)
{
float sin15 = 0.2588;
float sin30 = 0.5;
float sin45 = 0.7071;
float cos15 = 0.9659;
float cos30 = 0.866;
float cos45 = 0.7071;
switch (key)
{
case '0':
arduino.setArm3dCoordinates(ofVec3f(Arduino::ARM_LENGTH, 0, 0));
arduino.lookAt(ofVec3f(Arduino::ARM_LENGTH, -Arduino::KINECT_HEIGHT - Arduino::MOTORS_HEIGHT, 0.10));
break;
case '1':
//AngleMotor1 = -15
arduino.setArm3dCoordinates(ofVec3f(Arduino::ARM_LENGTH*cos15, -Arduino::ARM_LENGTH*sin15, 0));
arduino.lookAt(ofVec3f(arduino.getKinect3dCoordinates().x, arduino.getKinect3dCoordinates().y, arduino.getKinect3dCoordinates().z + 0.10));
break;
case '2':
//AngleMotor2 = 15
arduino.setArm3dCoordinates(ofVec3f(Arduino::ARM_LENGTH*cos15, 0, Arduino::ARM_LENGTH*sin15));
arduino.lookAt(ofVec3f(arduino.getKinect3dCoordinates().x, arduino.getKinect3dCoordinates().y, arduino.getKinect3dCoordinates().z + 0.10));
break;
case '3':
//AngleMotor8 > 90
arduino.setArm3dCoordinates(ofVec3f(Arduino::ARM_LENGTH, 0, 0));
arduino.lookAt(ofVec3f(arduino.getKinect3dCoordinates().x - 0.05, arduino.getKinect3dCoordinates().y, arduino.getKinect3dCoordinates().z + 0.10));
break;
case '4':
//AngleMotor4 < 0
arduino.setArm3dCoordinates(ofVec3f(Arduino::ARM_LENGTH, 0, 0));
arduino.lookAt(ofVec3f(arduino.getKinect3dCoordinates().x, arduino.getKinect3dCoordinates().y + 0.05, arduino.getKinect3dCoordinates().z + 0.15));
break;
case ' ':
storeCloud();
break;
case 'k':
saveRawClouds();
break;
case 'l':
loadRawClouds();
break;
}
}
else
{
static short editingValue = 1;
const float kValueChange = 0.002;
switch (key)
{
case 'z':
changeValue(Arduino::ARM_LENGTH, -kValueChange);
break;
case 'a':
changeValue(Arduino::ARM_LENGTH, kValueChange);
break;
case 'x':
changeValue(Arduino::KINECT_HEIGHT, -kValueChange);
break;
case 's':
changeValue(Arduino::KINECT_HEIGHT, kValueChange);
break;
case 'c':
changeValue(Arduino::MOTORS_HEIGHT, -kValueChange);
break;
case 'd':
changeValue(Arduino::MOTORS_HEIGHT, kValueChange);
break;
case ' ':
saveTransformedClouds();
break;
}
}
}
void CrichtonView<PointT>::fit_SQ( CloudPtr _cluster, int _index,
std::vector<SQ_parameters> &_ps,
ObjectEntry _oe ) {
CloudPtr filtered( new Cloud() );
CloudPtr completed( new Cloud() );
// Filter
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud (_cluster);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*filtered);
Eigen::Isometry3d Tsymm; Eigen::Vector3d Bb;
*completed = *filtered;
// Debug: Store raw cloud
char name[100]; sprintf( name, "%s_original", _oe.name.c_str() );
storeCloud( name, _index, completed );
// Mirror
clock_t ts, tf; double dt;
if( mMirror ) {
ts = clock();
mindGapper<PointT> mg;
std::vector<double> tc(4); for( int i = 0; i < 4; ++i ) { tc[i] = mTts.mTableCoeffs[i]; }
mg.setTablePlane( tc );
mg.setFittingParams();
mg.setDeviceParams();
mg.setFocalDist(mF);
mg.reset();
if( mg.complete( completed, true ) < 0 ) { printf("No mirror was valid \n"); sprintf( name, "%s_no_mirrored", _oe.name.c_str() ); }
else { printf("Mirroring was effectively applied \n"); sprintf( name, "%s_mirrored", _oe.name.c_str() ); }
tf = clock();
dt = (tf-ts)/(double) CLOCKS_PER_SEC;
printf("* Mirroring time: %f \n", dt );
// Debug: Store mirrored cloud
storeCloud( name, _index, completed );
}
// Fit superquadric
SQ_parameters p;
int type = SQ_FX_ICHIM; // MULTIPLE WE USE DIFFERENT
ts = clock();
switch( _oe.sq_type ) {
case REGULAR: {
printf("Type: regular \n");
SQ_fitter<PointT> fitter;
fitter.setInputCloud( completed );
fitter.fit( type, 0.03, 0.005, 5, 0.005 );
fitter.getFinalParams( p ); _ps.push_back(p);
break;
}
case TAMPERED: {
printf("Type: tampered \n");
SQ_fitter_t<PointT> fitter;
fitter.setInputCloud( completed );
fitter.fit( type, 0.03, 0.005, 5, 0.005 );
fitter.getFinalParams( p ); _ps.push_back(p);
break;
}
case BENT: {
printf("Type: bent \n");
SQ_fitter_b<PointT> fitter;
fitter.setInputCloud( completed );
fitter.fit( type, 0.03, 0.005, 5, 0.005 );
fitter.getFinalParams( p ); _ps.push_back(p);
break;
}
case MULTIPLE: {
printf("Type: multiple \n");
SQ_fitter_m<PointT> fitter;
fitter.setInputCloud( completed );
fitter.fit( SQ_FX_RADIAL, _oe.part_type, _oe.num_parts, _oe.hint_search, 0.03, 0.005, 5, 0.005 );
fitter.getFinalParams( _ps );
break;
}
}
tf = clock();
dt = (tf-ts)/(double)CLOCKS_PER_SEC;
printf("Fitting time");
if( mMirror ) { printf(" with mirror "); } else { printf(" with NO mirror: \n"); }
printf(" %f \n", dt );
if( _oe.sq_type != MULTIPLE ) {
printParamsInfo(_ps[0]);
pcl::PointCloud<pcl::PointXYZ>::Ptr sqp( new pcl::PointCloud<pcl::PointXYZ>() );
char sqname[75]; sprintf( sqname, "%s_sq.pcd", _oe.name.c_str() );
sqp = sampleSQ_uniform<pcl::PointXYZ>( _ps[0] );
pcl::io::savePCDFile( sqname, *sqp );
}
}
