void ransac3Dplane_distance(
const CMatrixTemplateNumeric<T> &allData,
const vector< CMatrixTemplateNumeric<T> > & testModels,
const T distanceThreshold,
unsigned int & out_bestModelIndex,
vector_size_t & out_inlierIndices )
{
ASSERT_( testModels.size()==1 )
out_bestModelIndex = 0;
const CMatrixTemplateNumeric<T> &M = testModels[0];
ASSERT_( size(M,1)==1 && size(M,2)==4 )
TPlane plane;
plane.coefs[0] = M(0,0);
plane.coefs[1] = M(0,1);
plane.coefs[2] = M(0,2);
plane.coefs[3] = M(0,3);
const size_t N = size(allData,2);
out_inlierIndices.clear();
out_inlierIndices.reserve(100);
for (size_t i=0;i<N;i++)
{
const double d = plane.distance( TPoint3D( allData.get_unsafe(0,i),allData.get_unsafe(1,i),allData.get_unsafe(2,i) ) );
if (d<distanceThreshold)
out_inlierIndices.push_back(i);
}
}
int main(int num_arg, char *argv[])
{
try
{
// Read function arguments
//----------------------------------------------------------------------
bool use_config_file = 0, enable_logfile = 0;
string filename;
if (num_arg < 2);
else if ( string(argv[1]) == "--help")
{
printf("\n\t Arguments of the function 'main' \n");
printf("==============================================================\n\n");
printf(" --help: Shows this menu... \n\n");
printf(" --config FICH.txt: Load FICH.txt as config file \n\n");
printf(" --create-config FICH.txt: Save the default config parameters \n\n");
printf(" \t\t\t in FICH.txt and close the program \n\n");
printf(" --save-logfile: Enable saving a log file with navigation data \n\n");
system::os::getch();
return 1;
}
else if ( string(argv[1]) == "--create-config")
{
filename = argv[2];
cout << endl << "Nombre del archivo: " << filename;
ofstream new_file(filename.c_str());
new_file << string(default_cfg_txt);
new_file.close();
cout << endl << "File saved" << endl;
system::os::getch();
return 1;
}
else
{
for (int i=1; i<num_arg; i++)
{
if ( string(argv[i]) == "--save-logfile")
enable_logfile = 1;
if ( string(argv[i]) == "--config")
{
use_config_file = 1;
filename = argv[i+1];
}
}
}
//Initial steps. Load configuration from file or default
//------------------------------------------------------
CMyReactInterface ReactInterface;
CReactiveNavigationSystem3D rn3d (ReactInterface, true, enable_logfile);
if (use_config_file == 0)
{
utils::CConfigFileMemory configNavigation(default_cfg_txt);
rn3d.loadConfigFile( configNavigation );
ReactInterface.loadMaps( configNavigation );
ReactInterface.loadConfiguration( configNavigation );
}
else
{
CConfigFile configNavigation(filename);
rn3d.loadConfigFile( configNavigation );
ReactInterface.loadMaps( configNavigation );
ReactInterface.loadConfiguration( configNavigation );
}
ReactInterface.initializeScene();
rn3d.initialize();
bool stop = 0;
bool moving_target = 0;
int pushed_key = 0;
TPoint3D last_Target_Pos(0,0,0);
CTicTac reactive_period;
reactive_period.Tic();
MyObserver observer;
observer.observeBegin(ReactInterface.window);
observer.mouse_click = 0;
while (!stop)
{
if (ReactInterface.window.keyHit())
pushed_key = ReactInterface.window.getPushedKey();
else
pushed_key = 0;
switch (pushed_key) {
case 'p':
//Pause navigation
rn3d.suspend();
break;
case 'r':
//Resume navigation
rn3d.resume();
break;
case 'm':
//Move the target
moving_target = 1;
break;
case 'e':
//Exit program
stop = 1;
break;
}
//Set the target when the user clicks the mouse
if (observer.mouse_click == 1)
{
observer.mouse_click = 0;
if (moving_target == 1)
{
moving_target = 0;
const CAbstractReactiveNavigationSystem::TNavigationParams nav_params = ReactInterface.createNewTarget(last_Target_Pos.x, last_Target_Pos.y, 0.3, 0);
rn3d.navigate(&nav_params);
}
}
//Execute navigation
rn3d.navigationStep();
ReactInterface.robotSim.simulateInterval( reactive_period.Tac() );
reactive_period.Tic();
if ((rn3d.IDLE == rn3d.getCurrentState())||(rn3d.SUSPENDED == rn3d.getCurrentState()))
{
CSimplePointsMap auxpoints;
ReactInterface.senseObstacles( auxpoints );
}
ReactInterface.updateScene();
system::sleep(5);
//Move target with the mouse
if (moving_target == 1)
{
int mouse_x,mouse_y;
if (ReactInterface.window.getLastMousePosition(mouse_x,mouse_y))
{
// Get the ray in 3D for the latest mouse (X,Y):
math::TLine3D ray;
ReactInterface.scene->getViewport("main")->get3DRayForPixelCoord(mouse_x,mouse_y,ray);
// Create a 3D plane, e.g. Z=0
const math::TPlane ground_plane(TPoint3D(0,0,0),TPoint3D(1,0,0),TPoint3D(0,1,0));
// Intersection of the line with the plane:
math::TObject3D inters;
math::intersect(ray,ground_plane, inters);
// Interpret the intersection as a point, if there is an intersection:
if (inters.getPoint(last_Target_Pos))
{
// Move an object to the position picked by the user:
ReactInterface.scene->getByClass<CDisk>(0)->setLocation(last_Target_Pos.x, last_Target_Pos.y, last_Target_Pos.z);
}
}
}
}
return 0;
}
catch (std::exception &e)
{
std::cout << "MRPT exception caught: " << e.what() << std::endl;
return -1;
}
catch (...)
{
printf("Untyped exception!!");
return -1;
}
}
