Exemple #1
0
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;
}