00001
00002
00003 #if !defined(FIDUCIALS_H_INCLUDED)
00004 #define FIDUCIALS_H_INCLUDED 1
00005
00006 typedef struct Fiducials__Struct *Fiducials;
00007 typedef struct Fiducials_Create__Struct *Fiducials_Create;
00008 typedef struct Fiducials_Results__Struct *Fiducials_Results;
00009
00010 #include <assert.h>
00011 #include <sys/time.h>
00012
00013 typedef void (*Fiducials_Arc_Announce_Routine)(void *announce_object,
00014 int from_id, double from_x, double from_y, double from_z,
00015 int to_id, double to_x, double to_y, double to_z,
00016 double goodness, bool in_spanning_tree);
00017
00018 typedef void (*Fiducials_Location_Announce_Routine)(void *announce_object,
00019 int id, double x, double y, double z, double bearing);
00020
00021 typedef void (*Fiducials_Tag_Announce_Routine)(void *announce_object,
00022 int id, double x, double y, double z, double twist,
00023 double diagonal, double distance_per_pixel,
00024 bool visible, int hop_count);
00025
00026 typedef void (*Fiducials_Fiducial_Announce_Routine)(void *announce_object,
00027 int id, int direction, double world_diagonal,
00028 double x1, double y1, double x2, double y2,
00029 double x3, double y3, double x4, double y4);
00030
00031
00032
00033 #include "CRC.hpp"
00034 #include "CV.hpp"
00035 #include "File.hpp"
00036 #include "FEC.hpp"
00037 #include "Map.hpp"
00038 #include "String.hpp"
00039 #include "Tag.hpp"
00040
00041 class CameraTag;
00042
00043 typedef bool Mapping[64];
00044 typedef struct timeval *Time_Value;
00045
00046 struct Fiducials__Struct {
00047 Fiducials_Arc_Announce_Routine arc_announce_routine;
00048 void *announce_object;
00049 CV_Scalar black;
00050 CV_Scalar blue;
00051 bool blur;
00052 std::vector<CameraTag*> camera_tags;
00053 CV_Point2D32F_Vector corners;
00054 std::vector<Tag*> current_visibles;
00055 CV_Scalar cyan;
00056 CV_Image debug_image;
00057 unsigned int debug_index;
00058 CV_Image edge_image;
00059 FEC fec;
00060 CV_Image gray_image;
00061 CV_Scalar green;
00062 CV_Size image_size;
00063 double last_x;
00064 double last_y;
00065 Fiducials_Location_Announce_Routine location_announce_routine;
00066 Fiducials_Fiducial_Announce_Routine fiducial_announce_routine;
00067 std::vector<Location*> locations;
00068 std::vector<Location*> locations_path;
00069 File log_file;
00070 Map map;
00071 CV_Point origin;
00072 CV_Image original_image;
00073 int **mappings;
00074 CV_Image map_x;
00075 CV_Image map_y;
00076 String_Const path;
00077 std::vector<Tag*> previous_visibles;
00078 CV_Scalar purple;
00079 CV_Scalar red;
00080 CV_Point2D32F_Vector references;
00081 Fiducials_Results results;
00082 CV_Point2D32F_Vector sample_points;
00083 unsigned int sequence_number;
00084 CV_Size size_5x5;
00085 CV_Size size_m1xm1;
00086 CV_Memory_Storage storage;
00087 Fiducials_Tag_Announce_Routine tag_announce_routine;
00088 bool tag_bits[64];
00089 CV_Image temporary_gray_image;
00090 CV_Term_Criteria term_criteria;
00091 unsigned int weights_index;
00092 bool y_flip;
00093 };
00094
00095 struct Fiducials_Create__Struct {
00096 String_Const fiducials_path;
00097 String_Const lens_calibrate_file_name;
00098 Memory announce_object;
00099 Fiducials_Arc_Announce_Routine arc_announce_routine;
00100 Fiducials_Location_Announce_Routine location_announce_routine;
00101 Fiducials_Tag_Announce_Routine tag_announce_routine;
00102 Fiducials_Fiducial_Announce_Routine fiducial_announce_routine;
00103 String_Const log_file_name;
00104 String_Const map_base_name;
00105 String_Const tag_heights_file_name;
00106 bool do_2d_slam;
00107 };
00108
00109 struct Fiducials_Results__Struct {
00110 bool map_changed;
00111 bool image_interesting;
00112 };
00113
00114 extern void Fiducials__arc_announce(void *announce_object,
00115 int from_id, double from_x, double from_y, double from_z,
00116 int to_id, double to_x, double to_y, double to_z,
00117 double goodness, bool in_spanning_tree);
00118 extern Fiducials Fiducials__create(
00119 CV_Image original_image, Fiducials_Create fiducials_create);
00120 extern void Fiducials__free(Fiducials fiduicals);
00121 extern void Fiducials__image_set(Fiducials fiducials, CV_Image image);
00122 extern void Fiducials__image_show(Fiducials fiducials, bool show);
00123 extern void Fiducials__location_announce(void *object, int id,
00124 double x, double y, double z, double bearing);
00125 extern int Fiducials__point_sample(
00126 Fiducials fiducials, CV_Point2D32F point);
00127 extern int Fiducials__points_maximum(Fiducials fiducials,
00128 CV_Point2D32F_Vector points, unsigned int start_index, unsigned int end_index);
00129 extern int Fiducials__points_minimum(Fiducials fiducials,
00130 CV_Point2D32F_Vector points, unsigned int start_index, unsigned int end_index);
00131 extern Fiducials_Results Fiducials__process(Fiducials fiducials);
00132 extern CV_Point2D32F_Vector Fiducials__references_compute(
00133 Fiducials fiducials, CV_Point2D32F_Vector corners);
00134 extern void Fiducials__sample_points_compute(
00135 CV_Point2D32F_Vector corners, CV_Point2D32F_Vector sample_points);
00136 extern void Fiducials__sample_points_helper(
00137 String_Const label, CV_Point2D32F corner, CV_Point2D32F sample_point);
00138 extern void Fiducials__tag_announce(void *announce_object,
00139 int id, double x, double y, double z, double twist,
00140 double diagonal, double distance_per_pixel,
00141 bool visible, int hop_count);
00142 extern Fiducials_Create Fiducials_Create__one_and_only(void);
00143
00144 #endif // !defined(FIDUCIALS_H_INCLUDED)