void ofxIcp::icp_step(const vector<cv::Point3d> & current, const vector<cv::Point3d> & target, vector<cv::Point3d> & output, cv::Mat & rotation, cv::Mat & translation, double & error){
vector<cv::Point3d> closest_current;
vector<cv::Point3d> closest_target;
find_closest_points(closest_points_count, current, target, closest_current, closest_target);
cv::Mat centroid_current;
cv::Mat centroid_target;
cv::reduce(cv::Mat(closest_current, false), centroid_current, 0, CV_REDUCE_AVG);
cv::reduce(cv::Mat(closest_target, false), centroid_target, 0, CV_REDUCE_AVG);
centroid_current = centroid_current.reshape(1);
centroid_target = centroid_target.reshape(1);
compute_rigid_transformation(closest_current, centroid_current, closest_target, centroid_target, rotation, translation);
vector<cv::Point3d> transformed_closest_current;
vector<cv::Point3d> transformed_current;
transform(closest_current, rotation, translation, transformed_closest_current);
transform(current, rotation, translation, transformed_current);
compute_error(transformed_closest_current, closest_target, error);
output = transformed_current;
}
int main( int argc, char *argv[] )
{
// Start SDL
if ( SDL_Init(SDL_INIT_EVERYTHING) < 0 )
{
printf( "SDL could not initialize! SDL Error: %s\n", SDL_GetError() );
return 1;
}
int result = 1;
point_t *points = NULL;
unsigned point_count;
size_t array_size;
// Parse From Text File
if ( argc > 1 )
{
FILE *file;
file = fopen( argv[1], "r" );
char buffer[256];
if ( file == NULL )
{
printf( "Unable to open file '%s'\n", argv[1] );
return 1;
}
if ( feof( file ) )
{
printf( "Empty File!" );
fclose( file );
return 1;
}
// SDL to time
clock_t end_time;
clock_t start_time = clock();
// Get Size Info and Allocate Memory
fgets( buffer, 256, file );
sscanf( buffer, "%u", &point_count );
array_size = point_count * sizeof( point_t );
printf( "Allocating %u bytes for point array (%u per point)\n",
array_size,
sizeof( point_t ) );
points = malloc( array_size );
// Read Points
for( unsigned i = 0; i < point_count; ++i )
{
//if ( feof( file ) )
if ( !fgets( buffer, 256, file ) )
{
point_count = i;
break;
}
//printf( "%s", buffer );
int read = sscanf( buffer, "(%lf, %lf)", &(points[i].x), &(points[i].y) );
//printf( "%i doubles read\n", read );
}
// Print file load time
end_time = clock();
printf( "\n%f seconds taken to load file\n", ((float)(end_time-start_time))/CLOCKS_PER_SEC );
start_time = end_time;
// Close File
fclose( file );
// Debug - Print out Points
if ( point_count <= MAX_PRINT )
print_points( points, point_count );
else
printf( "%u Points\nWay to many values to print!\n", point_count );
// Find and Print Closest Points
point_t close1, close2;
find_closest_points( points, point_count, &close1, &close2 );
printf( "Closest Points: (%lf,%lf) and (%lf,%lf)\n",
close1.x, close1.y, close2.x, close2.y );
end_time = clock();
printf( "\n%f seconds finding closest\n", ((float)(end_time-start_time))/CLOCKS_PER_SEC );
// Show GUI Solution
result = show_SDL_output();
}
else
{
printf( "Please pass a text file argument when running program!\n" );
return 1;
}
// Free Memory
if ( points != NULL )
free( points );
// Free SDL
SDL_Quit();
// Return Success
return result;
}
