diff --git a/README b/README deleted file mode 100644 index 60a5efb..0000000 --- a/README +++ /dev/null @@ -1,3 +0,0 @@ -See [ROS wiki](http://wiki.ros.org/ar_track_alvar) for the users document. - - diff --git a/README.md b/README.md new file mode 100644 index 0000000..cd23649 --- /dev/null +++ b/README.md @@ -0,0 +1,39 @@ +See [ROS wiki](http://wiki.ros.org/ar_track_alvar) for the users document. + +# ar_track_alvar + +### Convention +We always consider the orientation shown below as the reference orientation of the mark. You can distinguish this orientation with the two empty squares represented by the red box on this example. + + + +When you create a new tag, an XML file will be generated with it. On this file you will find the description of the corners of each tag like this one: +``` + + + + +``` +If you want to manually modify this file, assume that the corners are as follows. + + + +Finally, we will consider the following 3D marker for each tag. + + + +### Identifiers +Each tags represents an identifier. In these nodes, the identifiers are numbers between 0 and 65535. +The identifiers used to create a tag will be specified in the name of the PNG, a generated XML file. + +e.g : +For a single marker : +``` +MarkerData_0.png +MarkerData_0.xml +``` +For a cube : +``` +MarkerData_1_2_3_4_5_6.png +MarkerData_1_2_3_4_5_6.xml +``` diff --git a/ar_track_alvar/CMakeLists.txt b/ar_track_alvar/CMakeLists.txt index 7450ac6..3eed9b0 100644 --- a/ar_track_alvar/CMakeLists.txt +++ b/ar_track_alvar/CMakeLists.txt @@ -65,7 +65,7 @@ catkin_package( dynamic_reconfigure ) -include_directories(include +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} @@ -77,6 +77,7 @@ set(GENCPP_DEPS ar_track_alvar_msgs_gencpp std_msgs_gencpp sensor_msgs_gencpp ge add_library(ar_track_alvar src/Camera.cpp src/CaptureDevice.cpp + src/Color.cpp src/Pose.cpp src/Marker.cpp src/MarkerDetector.cpp @@ -145,6 +146,14 @@ add_executable(createMarker src/SampleMarkerCreator.cpp) target_link_libraries(createMarker ar_track_alvar ${catkin_LIBRARIES}) add_dependencies(createMarker ${PROJECT_NAME}_gencpp ${GENCPP_DEPS}) +add_executable(createCube src/CubeCreator.cpp) +target_link_libraries(createCube ar_track_alvar ${catkin_LIBRARIES}) +add_dependencies(createCube ${PROJECT_NAME}_gencpp ${GENCPP_DEPS}) + +add_executable(createDeportedMarker src/DeportedMarkerCreator.cpp) +target_link_libraries(createDeportedMarker ar_track_alvar ${catkin_LIBRARIES}) +add_dependencies(createDeportedMarker ${PROJECT_NAME}_gencpp ${GENCPP_DEPS}) + install(TARGETS ${ALVAR_TARGETS} ${KINECT_FILTERING_TARGETS} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -161,12 +170,12 @@ install(DIRECTORY launch bundles if (CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roslaunch rostest) - + file(GLOB LAUNCH_FILES launch/*.launch test/*.test) foreach(LAUNCH_FILE ${LAUNCH_FILES}) roslaunch_add_file_check(${LAUNCH_FILE} USE_TEST_DEPENDENCIES) endforeach() - + catkin_download_test_data( ${PROJECT_NAME}_4markers_tork.bag http://download.ros.org/data/ar_track_alvar/ar_track_alvar_4markers_tork_2017-02-08-11-21-14.bag diff --git a/ar_track_alvar/Marker_creation.md b/ar_track_alvar/Marker_creation.md new file mode 100644 index 0000000..14a533b --- /dev/null +++ b/ar_track_alvar/Marker_creation.md @@ -0,0 +1,76 @@ +# Marker creation + +## Deported marker generation +A tag called "Deported marker" is a single 2D marker that refers to a position offset to its own. + +### Usage +``` +$ rosrun ar_track_alvar createDeportedMarker +``` +First, enter the ID you want to use. The identifiers are chosen by you between 0 and 65535. Be careful, all the tags (deported or not), **must have a different identifier**, that is why the node can automatically increases the identifiers of tags. + +The reference unit use on this node is the centimeter but you can change it with option '-u'. +After specifying the size of the tag, you can describe the position offset on X, Y, and Z. + + + +### Tips +- Always take 1 cm margin on the size of your tag to keep a white border for better detection. +- Be carefull about the tag orientation + +## Cube generation +A tag called "Cube" is composed of 6 tags assembled in a cube. Only the master tag will be displayed regardless of the orientation of the cube. + +### Usage +``` +$ rosrun ar_track_alvar createCube +``` +First, enter the ID you want to use. The identifiers are chosen by you between 0 and 65535. Be careful, all the tags (deported or not), **must have a different identifier**, that is why the node can automatically increases the identifiers of tags (**+6 per cube**). The master identifier will always be the first one present in the PNG and XML files name. + +The reference unit use on this node is the centimeter but you can change it with option '-u'. +After specifying the size of the tag, specified the size of the cube and ... this is all you need to do !! + +The node will generate a pattern that you have to cut and stick. + :exclamation: Once the PNG is printed, do not cut all markers independently, but only the entire pattern. + +### Tips +- Always take 1 cm margin on the size of your tag to keep a white border for better detection. +- If you have to cut all the tags of the pattern, make sure to stick them with the correct order and orientation. + +## Colors + +For deported markers and cubes, you can add color. Six colors are available in addition to black !! + + + +## Options + +Several options can be added when you run the generators. + +### For deported markers + + - **65535** marker with number 65535 + - **-1 \"hello world\"** marker with string + - **-2 catalog.xml** marker with file reference + - **-3 http://wiki.ros.org** marker with URL + - **-u 96** use units corresponding to 1.0 unit per 96 pixels + - **-uin** use inches as units (assuming 96 dpi) + - **-ucm** use cm's as units (assuming 96 dpi) + - **-s 5.0** use marker size 5.0x5.0 units (default 9.0x9.0) + - **-r 5** marker content resolution -- 0 uses default + - **-m 2.0** marker margin resolution -- 0 uses default + - **-p** prompt marker placements interactively from the user + - **-c B** marker color : B:blue R:red G:green P:pink S:sky Y:yellow -- default black + +### For cubes + + - **65535** marker with number 65535 + - **-u 96** use units corresponding to 1.0 unit per 96 pixels + - **-uin** use inches as units (assuming 96 dpi) + - **-ucm** use cm's as units (assuming 96 dpi) + - **-sm 5.0** use marker size 5.0x5.0 units (default 9.0x9.0) + - **-sc 5.0** use cube size 5.0x5.0 units (default 9.0x9.0) + - **-r 5** marker content resolution -- 0 uses default + - **-m 1.0** marker margin resolution -- 0 uses default + - **-p** prompt marker placements interactively from the user + - **-c B** cube color : B:blue R:red G:green P:pink S:sky Y:yellow -- default black diff --git a/ar_track_alvar/include/ar_track_alvar/Color.h b/ar_track_alvar/include/ar_track_alvar/Color.h new file mode 100644 index 0000000..72dcdc6 --- /dev/null +++ b/ar_track_alvar/include/ar_track_alvar/Color.h @@ -0,0 +1,24 @@ +#include "ar_track_alvar/MultiMarker.h" +#include "highgui.h" +using namespace std; +using namespace alvar; + +namespace col +{ + enum color_t + { + COLOR_BLACK, + COLOR_RED, + COLOR_BLUE, + COLOR_GREEN, + COLOR_YELLOW, + COLOR_PINK, + COLOR_SKY + }; + + color_t get_color(char color_name); + + unsigned char get_color(color_t p_color); + + void change_color(IplImage *p_img, unsigned char p_color = 0); +} diff --git a/ar_track_alvar/include/ar_track_alvar/Marker.h b/ar_track_alvar/include/ar_track_alvar/Marker.h index 0313833..9cda29d 100644 --- a/ar_track_alvar/include/ar_track_alvar/Marker.h +++ b/ar_track_alvar/include/ar_track_alvar/Marker.h @@ -58,17 +58,17 @@ namespace alvar { bool UpdateContentBasic(std::vector > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no = 0); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool valid; - /** \brief Compares the marker corners with the previous match. + /** \brief Compares the marker corners with the previous match. * * In some cases the tracking of the marker can be accepted solely based on this. - * Returns the marker orientation and an error value describing the pixel error + * Returns the marker orientation and an error value describing the pixel error * relative to the marker diameter. */ void CompareCorners(std::vector > &_marker_corners_img, int *orientation, double *error); - /** \brief Compares the marker corners with the previous match. + /** \brief Compares the marker corners with the previous match. */ void CompareContent(std::vector > &_marker_corners_img, IplImage *gray, Camera *cam, int *orientation) const; /** \brief Updates the \e marker_content from the image using \e Homography @@ -77,11 +77,11 @@ namespace alvar { /** \brief Updates the markers \e pose estimation */ void UpdatePose(std::vector > &_marker_corners_img, Camera *cam, int orientation, int frame_no = 0, bool update_pose = true); - /** \brief Decodes the marker content. Please call \e UpdateContent before this. + /** \brief Decodes the marker content. Please call \e UpdateContent before this. * This virtual method is meant to be implemented by heirs. */ virtual bool DecodeContent(int *orientation); - + /** \brief Returns the content as a matrix */ CvMat *GetContent() const { @@ -101,10 +101,10 @@ namespace alvar { /** \brief Get edge length (to support different size markers */ double GetMarkerEdgeLength() const { return edge_length; } /** \brief Destructor */ - ~Marker(); - /** \brief Default constructor + virtual ~Marker(); + /** \brief Default constructor * \param _edge_length Length of the marker's edge in whatever units you are using (e.g. cm) - * \param _res The marker content resolution in pixels (this is actually + * \param _res The marker content resolution in pixels (this is actually * \param _margin The marker margin resolution in pixels (The actual captured marker image has pixel resolution of _margin+_res+_margin) */ Marker(double _edge_length = 0, int _res = 0, double _margin = 0); @@ -119,7 +119,7 @@ namespace alvar { virtual void SetId(unsigned long _id) {}; /** - * Returns the resolution (the number of square rows and columns) of the + * Returns the resolution (the number of square rows and columns) of the * marker content area. The total number of content squares within the * content area is resolution*resolution. */ @@ -168,7 +168,7 @@ namespace alvar { CvMat *marker_content; public: - + /** \brief Marker color points in marker coordinates */ std::vector marker_points; /** \brief Marker corners in marker coordinates */ @@ -198,11 +198,11 @@ namespace alvar { double default_margin() { return 1.5; } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** \brief \e MarkerArtoolkit supports only 'id' as data type */ unsigned long id; /** \brief Constructor */ - MarkerArtoolkit(double _edge_length = 0, int _res = 0, double _margin = 0) : + MarkerArtoolkit(double _edge_length = 0, int _res = 0, double _margin = 0) : Marker(_edge_length, (_res?_res:3), (_margin?_margin:1.5)) { } @@ -230,7 +230,7 @@ namespace alvar { bool DetectResolution(std::vector > &_marker_corners_img, IplImage *gray, Camera *cam); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EIGEN_MAKE_ALIGNED_OPERATOR_NEW static const int MAX_MARKER_STRING_LEN=2048; enum MarkerContentType { MARKER_CONTENT_TYPE_NUMBER, @@ -242,16 +242,16 @@ namespace alvar { /** \brief \e MarkerData content can be presented either as number (\e MARKER_CONTENT_TYPE_NUMBER) or string */ union { - unsigned long id; - char str[MAX_MARKER_STRING_LEN]; + unsigned long id; + char str[MAX_MARKER_STRING_LEN]; } data; - /** \brief Default constructor + /** \brief Default constructor * \param _edge_length Length of the marker's edge in whatever units you are using (e.g. cm) - * \param _res The marker content resolution in pixels (this is actually + * \param _res The marker content resolution in pixels (this is actually * \param _margin The marker margin resolution in pixels (The actual captured marker image has pixel resolution of _margin+_res+_margin) */ - MarkerData(double _edge_length = 0, int _res = 0, double _margin = 0) : + MarkerData(double _edge_length = 0, int _res = 0, double _margin = 0) : Marker(_edge_length, _res, (_margin?_margin:2)) { } @@ -260,11 +260,11 @@ namespace alvar { /** \brief Set the ID */ void SetId(unsigned long _id) { data.id = _id; } /** \brief Updates the \e marker_content from the image using \e Homography - * Compared to the basic implementation in \e Marker this will also detect the marker + * Compared to the basic implementation in \e Marker this will also detect the marker * resolution automatically when the marker resolution is specified to be 0. */ virtual bool UpdateContent(std::vector > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no = 0); - /** \brief \e DecodeContent should be called after \e UpdateContent to fill \e content_type, \e decode_error and \e data + /** \brief \e DecodeContent should be called after \e UpdateContent to fill \e content_type, \e decode_error and \e data */ bool DecodeContent(int *orientation); /** \brief Updates the \e marker_content by "encoding" the given parameters @@ -293,7 +293,7 @@ namespace alvar { template class ALVAR_EXPORT MarkerIteratorImpl : public MarkerIterator { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef typename std::vector >::const_iterator const_iterator_aligntype; MarkerIteratorImpl(const_iterator_aligntype i) : _begin(i), _i(i) { _data = this; diff --git a/ar_track_alvar/include/ar_track_alvar/MultiMarker.h b/ar_track_alvar/include/ar_track_alvar/MultiMarker.h index ade1d2d..f0a0b4a 100644 --- a/ar_track_alvar/include/ar_track_alvar/MultiMarker.h +++ b/ar_track_alvar/include/ar_track_alvar/MultiMarker.h @@ -55,9 +55,9 @@ class ALVAR_EXPORT MultiMarker { bool LoadXML(const char* fname); public: - // The marker information is stored in all three tables using - // the indices-order given in constructor. - // One idea is that the same 'pointcloud' could contain feature + // The marker information is stored in all three tables using + // the indices-order given in constructor. + // One idea is that the same 'pointcloud' could contain feature // points after marker-corner-points. This way they would be // optimized simultaneously with marker corners... std::map pointcloud; @@ -71,7 +71,7 @@ class ALVAR_EXPORT MultiMarker { double _GetPose(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image); int _SetTrackMarkers(MarkerDetectorImpl &marker_detector, Camera* cam, Pose& pose, IplImage *image); - int master_id; //The id of the first marker specified in the XML file + int master_id; //The id of the first marker specified in the XML file /** \brief Resets the multi marker. */ @@ -97,7 +97,7 @@ class ALVAR_EXPORT MultiMarker { /** \brief Default constructor */ MultiMarker() {} - /** \brief Calculates the pose of the camera from multi marker. Method uses the true 3D coordinates of + /** \brief Calculates the pose of the camera from multi marker. Method uses the true 3D coordinates of markers to get the initial pose and then optimizes it by minimizing the reprojection error. \param markers Vector of markers seen by camera. @@ -143,6 +143,18 @@ class ALVAR_EXPORT MultiMarker { */ void PointCloudAdd(int marker_id, double edge_length, Pose &pose); + /** \brief Translate all corner in 3D space + \param marker_id Id of the marker to be translate. + \param x,y,z Values of translation + */ + void PointCloudTranslate(int marker_id, double x, double y, double z); + + /**\brief Rotate all corner in 3D space + \param marker_id Id of the marker to be rotate. + \param rot matrix of rotation + */ + void PointCloudRotate(int marker_id, int rot[9]); + /** \brief Copies the 3D point cloud from other multi marker object. */ void PointCloudCopy(const MultiMarker *m); diff --git a/ar_track_alvar/nodes/FindMarkerBundles.cpp b/ar_track_alvar/nodes/FindMarkerBundles.cpp index e7f83e0..914d30f 100644 --- a/ar_track_alvar/nodes/FindMarkerBundles.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundles.cpp @@ -99,7 +99,7 @@ Pose *bundlePoses; int *master_id; int *bundles_seen; bool *master_visible; -std::vector *bundle_indices; +std::vector *bundle_indices; bool init = true; ata::MedianFilter **med_filts; int med_filt_size; @@ -107,10 +107,10 @@ int med_filt_size; double marker_size; double max_new_marker_error; double max_track_error; -std::string cam_image_topic; -std::string cam_info_topic; +std::string cam_image_topic; +std::string cam_info_topic; std::string output_frame; -int n_bundles = 0; +int n_bundles = 0; //Debugging utility function void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad) @@ -118,17 +118,17 @@ void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double ra visualization_msgs::Marker rvizMarker; rvizMarker.header.frame_id = frame; - rvizMarker.header.stamp = ros::Time::now(); + rvizMarker.header.stamp = ros::Time::now(); rvizMarker.id = id; rvizMarker.ns = "3dpts"; - + rvizMarker.scale.x = rad; rvizMarker.scale.y = rad; rvizMarker.scale.z = rad; - + rvizMarker.type = visualization_msgs::Marker::SPHERE_LIST; rvizMarker.action = visualization_msgs::Marker::ADD; - + if(color==1){ rvizMarker.color.r = 0.0f; rvizMarker.color.g = 1.0f; @@ -147,15 +147,15 @@ void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double ra rvizMarker.color.b = 0.0f; rvizMarker.color.a = 1.0; } - + gm::Point p; - for(int i=0; ipoints.size(); i++){ + for(size_t i=0; ipoints.size(); i++){ p.x = cloud->points[i].x; p.y = cloud->points[i].y; p.z = cloud->points[i].z; rvizMarker.points.push_back(p); } - + rvizMarker.lifetime = ros::Duration (1.0); rvizMarkerPub2_.publish (rvizMarker); } @@ -164,21 +164,21 @@ void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double ra void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int id) { visualization_msgs::Marker rvizMarker; - + rvizMarker.header.frame_id = frame; - rvizMarker.header.stamp = ros::Time::now(); + rvizMarker.header.stamp = ros::Time::now(); rvizMarker.id = id; rvizMarker.ns = "arrow"; - + rvizMarker.scale.x = 0.01; rvizMarker.scale.y = 0.01; rvizMarker.scale.z = 0.1; - + rvizMarker.type = visualization_msgs::Marker::ARROW; rvizMarker.action = visualization_msgs::Marker::ADD; - - for(int i=0; i<3; i++){ - rvizMarker.points.clear(); + + for(size_t i=0; i<3; i++){ + rvizMarker.points.clear(); rvizMarker.points.push_back(start); gm::Point end; end.x = start.x + mat[0][i]; @@ -214,11 +214,11 @@ void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int // Infer the master tag corner positons from the other observed tags -// Also does some of the bookkeeping for tracking that MultiMarker::_GetPose does +// Also does some of the bookkeeping for tracking that MultiMarker::_GetPose does int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_corners){ bund_corners.clear(); bund_corners.resize(4); - for(int i=0; i<4; i++){ + for(size_t i=0; i<4; i++){ bund_corners[i].x = 0; bund_corners[i].y = 0; bund_corners[i].z = 0; @@ -237,7 +237,6 @@ int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_ const Marker* marker = &((*marker_detector.markers)[i]); int id = marker->GetId(); int index = master.get_id_index(id); - int mast_id = master.master_id; if (index < 0) continue; // But only if we have corresponding points in the pointcloud @@ -256,13 +255,13 @@ int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_ tf::Vector3 corner_coord = master.rel_corners[index][j]; gm::PointStamped p, output_p; p.header.frame_id = marker_frame; - p.point.x = corner_coord.y()/100.0; + p.point.x = corner_coord.y()/100.0; p.point.y = -corner_coord.x()/100.0; p.point.z = corner_coord.z()/100.0; - + try{ tf_listener->waitForTransform(cloud.header.frame_id, marker_frame, ros::Time(0), ros::Duration(0.1)); - tf_listener->transformPoint(cloud.header.frame_id, p, output_p); + tf_listener->transformPoint(cloud.header.frame_id, p, output_p); } catch (tf::TransformException ex){ ROS_ERROR("ERROR InferCorners: %s",ex.what()); @@ -276,10 +275,10 @@ int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_ master.marker_status[index] = 2; // Used for tracking } } - + //Divide to take the average of the summed estimates if(n_est > 0){ - for(int i=0; i<4; i++){ + for(size_t i=0; i<4; i++){ bund_corners[i].x /= n_est; bund_corners[i].y /= n_est; bund_corners[i].z /= n_est; @@ -299,13 +298,13 @@ int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr sele pose.pose.position = ata::centroid(*res.inliers); draw3dPoints(selected_points, cloud.header.frame_id, 1, id, 0.005); - - //Get 2 points that point forward in marker x direction + + //Get 2 points that point forward in marker x direction int i1,i2; - if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) || + if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) || isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z)) { - if(isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z) || + if(isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z) || isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z)) { return -1; @@ -313,41 +312,41 @@ int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr sele else{ i1 = 1; i2 = 2; - } + } } else{ i1 = 0; i2 = 3; } - //Get 2 points the point forward in marker y direction + //Get 2 points the point forward in marker y direction int i3,i4; - if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) || + if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) || isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z)) - { - if(isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z) || + { + if(isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z) || isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z)) - { + { return -1; } else{ i3 = 2; i4 = 3; - } + } } else{ i3 = 1; i4 = 0; } - + ARCloud::Ptr orient_points(new ARCloud()); orient_points->points.push_back(corners_3D[i1]); draw3dPoints(orient_points, cloud.header.frame_id, 3, id+1000, 0.008); - + orient_points->clear(); orient_points->points.push_back(corners_3D[i2]); draw3dPoints(orient_points, cloud.header.frame_id, 2, id+2000, 0.008); - + int succ; succ = ata::extractOrientation(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], pose.pose.orientation); if(succ < 0) return -1; @@ -364,7 +363,7 @@ int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr sele p.quaternion[1] = pose.pose.orientation.x; p.quaternion[2] = pose.pose.orientation.y; p.quaternion[3] = pose.pose.orientation.z; - p.quaternion[0] = pose.pose.orientation.w; + p.quaternion[0] = pose.pose.orientation.w; return 0; } @@ -378,10 +377,10 @@ void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) { master_visible[i] = false; bundles_seen[i] = 0; } - + //Detect and track the markers if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, - max_track_error, CVSEQ, true)) + max_track_error, CVSEQ, true)) { //printf("\n--------------------------\n\n"); for (size_t i=0; isize(); i++) @@ -390,7 +389,7 @@ void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) { Marker *m = &((*marker_detector.markers)[i]); int id = m->GetId(); //cout << "******* ID: " << id << endl; - + //Get the 3D points of the outer corners /* PointDouble corner0 = m->marker_corners_img[0]; @@ -402,22 +401,22 @@ void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) { m->ros_corners_3D[2] = cloud(corner2.x, corner2.y); m->ros_corners_3D[3] = cloud(corner3.x, corner3.y); */ - + //Get the 3D inner corner points - more stable than outer corners that can "fall off" object int resol = m->GetRes(); int ori = m->ros_orientation; - + PointDouble pt1, pt2, pt3, pt4; pt4 = m->ros_marker_points_img[0]; pt3 = m->ros_marker_points_img[resol-1]; pt1 = m->ros_marker_points_img[(resol*resol)-resol]; pt2 = m->ros_marker_points_img[(resol*resol)-1]; - + m->ros_corners_3D[0] = cloud(pt1.x, pt1.y); m->ros_corners_3D[1] = cloud(pt2.x, pt2.y); m->ros_corners_3D[2] = cloud(pt3.x, pt3.y); m->ros_corners_3D[3] = cloud(pt4.x, pt4.y); - + if(ori >= 0 && ori < 4){ if(ori != 0){ std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end()); @@ -430,14 +429,14 @@ void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) { int master_ind = -1; for(int j=0; jros_marker_points_img) - pixels.push_back(cv::Point(p.x, p.y)); + pixels.push_back(cv::Point(p.x, p.y)); ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels); //Use the kinect data to find a plane and pose for the marker int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose); - + //If the plane fit fails... if(ret < 0){ //Mark this tag as invalid @@ -463,14 +462,14 @@ void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) { master_visible[master_ind] = false; //decrement the number of markers seen in this bundle bundles_seen[bundle_ind] -= 1; - + } else m->valid = true; - } + } //For each master tag, infer the 3D position of its corners from other visible tags - //Then, do a plane fit to those new corners + //Then, do a plane fit to those new corners ARCloud inferred_corners; for(int i=0; i 0){ @@ -483,27 +482,27 @@ void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) { //If master is visible, use it directly instead of inferring pose //else{ // for (size_t j=0; jsize(); j++){ - // Marker *m = &((*marker_detector.markers)[j]); + // Marker *m = &((*marker_detector.markers)[j]); // if(m->GetId() == master_id[i]) // bundlePoses[i] = m->pose; - // } + // } //} Pose ret_pose; if(med_filt_size > 0){ med_filts[i]->addPose(bundlePoses[i]); med_filts[i]->getMedian(ret_pose); bundlePoses[i] = ret_pose; - } - } + } + } } } } -// Given the pose of a marker, builds the appropriate ROS messages for later publishing +// Given the pose of a marker, builds the appropriate ROS messages for later publishing void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker, int confidence){ double px,py,pz,qx,qy,qz,qw; - + px = p.translation[0]/100.0; py = p.translation[1]/100.0; pz = p.translation[2]/100.0; @@ -571,7 +570,7 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ rvizMarker->lifetime = ros::Duration (0.1); - // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization + // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization if(type==MAIN_MARKER){ //Take the pose of the tag in the camera frame and convert to the output frame (usually torso_lift_link for the PR2) tf::Transform tagPoseOutput = CamToOutput * markerPose; @@ -612,7 +611,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) ar_track_alvar_msgs::AlvarMarker ar_pose_marker; arPoseMarkers_.markers.clear (); - //Convert cloud to PCL + //Convert cloud to PCL ARCloud cloud; pcl::fromROSMsg(*msg, cloud); @@ -620,7 +619,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) pcl::toROSMsg (*msg, *image_msg); image_msg->header.stamp = msg->header.stamp; image_msg->header.frame_id = msg->header.frame_id; - + //Convert the image cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8); @@ -634,7 +633,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) for (size_t i=0; isize(); i++) { - int id = (*(marker_detector.markers))[i].GetId(); + int id = (*(marker_detector.markers))[i].GetId(); // Draw if id is valid if(id >= 0){ @@ -651,7 +650,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) } } } - + //Draw the main markers, whether they are visible or not -- but only if at least 1 marker from their bundle is currently seen for(int i=0; i eig_quat(eig_m); - + // Translate back to bullet tfScalar ex = eig_quat.x(); tfScalar ey = eig_quat.y(); @@ -711,15 +710,15 @@ int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1, tfScalar ew = eig_quat.w(); tf::Quaternion quat(ex,ey,ez,ew); quat = quat.normalized(); - + double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0; double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0; double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0; tf::Vector3 origin (qx,qy,qz); - + tf::Transform tform (quat, origin); //transform from master to marker retT = tform; - + return 0; } @@ -730,38 +729,38 @@ int calcAndSaveMasterCoords(MultiMarkerBundle &master) { int mast_id = master.master_id; std::vector rel_corner_coords; - + //Go through all the markers associated with this bundle for (size_t i=0; i[n_bundles]; - bundles_seen = new int[n_bundles]; + master_id = new int[n_bundles]; + bundle_indices = new std::vector[n_bundles]; + bundles_seen = new int[n_bundles]; master_visible = new bool[n_bundles]; - + //Create median filters med_filts = new ata::MedianFilter*[n_bundles]; for(int i=0; i id_vector = loadHelper.getIndices(); - multi_marker_bundles[i] = new MultiMarkerBundle(id_vector); + multi_marker_bundles[i] = new MultiMarkerBundle(id_vector); multi_marker_bundles[i]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML); master_id[i] = multi_marker_bundles[i]->getMasterId(); bundle_indices[i] = multi_marker_bundles[i]->getIndices(); calcAndSaveMasterCoords(*(multi_marker_bundles[i])); } else{ - cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl; + cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl; return 0; - } - } + } + } // Set up camera, listeners, and broadcasters cam = new Camera(n, cam_info_topic); @@ -828,11 +827,11 @@ int main(int argc, char *argv[]) arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0); rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0); rvizMarkerPub2_ = n.advertise < visualization_msgs::Marker > ("ARmarker_points", 0); - + //Give tf a chance to catch up before the camera callback starts asking for transforms ros::Duration(1.0).sleep(); - ros::spinOnce(); - + ros::spinOnce(); + //Subscribe to topics and set up callbacks ROS_INFO ("Subscribing to image topic"); cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback); @@ -841,5 +840,3 @@ int main(int argc, char *argv[]) return 0; } - - diff --git a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp index 013588b..06b1d14 100644 --- a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp @@ -67,16 +67,16 @@ MultiMarkerBundle **multi_marker_bundles=NULL; Pose *bundlePoses; int *master_id; bool *bundles_seen; -std::vector *bundle_indices; -bool init = true; +std::vector *bundle_indices; +bool init = true; double marker_size; double max_new_marker_error; double max_track_error; -std::string cam_image_topic; -std::string cam_info_topic; +std::string cam_image_topic; +std::string cam_info_topic; std::string output_frame; -int n_bundles = 0; +int n_bundles = 0; void GetMultiMarkerPoses(IplImage *image); void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg); @@ -89,7 +89,7 @@ void GetMultiMarkerPoses(IplImage *image) { if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)){ for(int i=0; iUpdate(marker_detector.markers, cam, bundlePoses[i]); - + if(marker_detector.DetectAdditional(image, cam, false) > 0){ for(int i=0; iSetTrackMarkers(marker_detector, cam, bundlePoses[i], image) > 0)) @@ -100,10 +100,10 @@ void GetMultiMarkerPoses(IplImage *image) { } -// Given the pose of a marker, builds the appropriate ROS messages for later publishing +// Given the pose of a marker, builds the appropriate ROS messages for later publishing void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker){ double px,py,pz,qx,qy,qz,qw; - + px = p.translation[0]/100.0; py = p.translation[1]/100.0; pz = p.translation[2]/100.0; @@ -173,7 +173,7 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ rvizMarker->lifetime = ros::Duration (1.0); - // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization + // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization if(type==MAIN_MARKER){ //Take the pose of the tag in the camera frame and convert to the output frame (usually torso_lift_link for the PR2) tf::Transform tagPoseOutput = CamToOutput * markerPose; @@ -220,7 +220,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) // do this conversion here -jbinney IplImage ipl_image = cv_ptr_->image; GetMultiMarkerPoses(&ipl_image); - + //Draw the observed markers that are visible and note which bundles have at least 1 marker seen for(int i=0; i[n_bundles]; - bundles_seen = new bool[n_bundles]; + master_id = new int[n_bundles]; + bundle_indices = new std::vector[n_bundles]; + bundles_seen = new bool[n_bundles]; // Load the marker bundle XML files - for(int i=0; i id_vector = loadHelper.getIndices(); - multi_marker_bundles[i] = new MultiMarkerBundle(id_vector); + multi_marker_bundles[i] = new MultiMarkerBundle(id_vector); multi_marker_bundles[i]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML); master_id[i] = multi_marker_bundles[i]->getMasterId(); bundle_indices[i] = multi_marker_bundles[i]->getIndices(); } else{ - cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl; + cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl; return 0; - } - } + } + } // Set up camera, listeners, and broadcasters cam = new Camera(n, cam_info_topic); @@ -329,11 +329,11 @@ int main(int argc, char *argv[]) tf_broadcaster = new tf::TransformBroadcaster(); arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0); rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0); - + //Give tf a chance to catch up before the camera callback starts asking for transforms ros::Duration(1.0).sleep(); - ros::spinOnce(); - + ros::spinOnce(); + //Subscribe to topics and set up callbacks ROS_INFO ("Subscribing to image topic"); image_transport::ImageTransport it_(n); diff --git a/ar_track_alvar/nodes/IndividualMarkers.cpp b/ar_track_alvar/nodes/IndividualMarkers.cpp index a44a23f..6db3a3e 100644 --- a/ar_track_alvar/nodes/IndividualMarkers.cpp +++ b/ar_track_alvar/nodes/IndividualMarkers.cpp @@ -124,7 +124,7 @@ void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double ra } gm::Point p; - for(int i=0; ipoints.size(); i++){ + for(size_t i=0; ipoints.size(); i++){ p.x = cloud->points[i].x; p.y = cloud->points[i].y; p.z = cloud->points[i].z; @@ -152,7 +152,7 @@ void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int rvizMarker.type = visualization_msgs::Marker::ARROW; rvizMarker.action = visualization_msgs::Marker::ADD; - for(int i=0; i<3; i++){ + for(size_t i=0; i<3; i++){ rvizMarker.points.clear(); rvizMarker.points.push_back(start); gm::Point end; @@ -277,41 +277,41 @@ void GetMarkerPoses(IplImage *image, ARCloud &cloud) { ROS_DEBUG_STREAM("--------------------------"); for (size_t i=0; isize(); i++) { - vector > pixels; - Marker *m = &((*marker_detector.markers)[i]); - int id = m->GetId(); - ROS_DEBUG_STREAM("******* ID: " << id); - - int resol = m->GetRes(); - int ori = m->ros_orientation; - - PointDouble pt1, pt2, pt3, pt4; - pt4 = m->ros_marker_points_img[0]; - pt3 = m->ros_marker_points_img[resol-1]; - pt1 = m->ros_marker_points_img[(resol*resol)-resol]; - pt2 = m->ros_marker_points_img[(resol*resol)-1]; - - m->ros_corners_3D[0] = cloud(pt1.x, pt1.y); - m->ros_corners_3D[1] = cloud(pt2.x, pt2.y); - m->ros_corners_3D[2] = cloud(pt3.x, pt3.y); - m->ros_corners_3D[3] = cloud(pt4.x, pt4.y); - - if(ori >= 0 && ori < 4){ - if(ori != 0){ - std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end()); - } - } - else - ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id); - - //Get the 3D marker points - BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img) - pixels.push_back(cv::Point(p.x, p.y)); - ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels); - - //Use the kinect data to find a plane and pose for the marker - int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose); - } + vector > pixels; + Marker *m = &((*marker_detector.markers)[i]); + int id = m->GetId(); + ROS_DEBUG_STREAM("******* ID: " << id); + + int resol = m->GetRes(); + int ori = m->ros_orientation; + + PointDouble pt1, pt2, pt3, pt4; + pt4 = m->ros_marker_points_img[0]; + pt3 = m->ros_marker_points_img[resol-1]; + pt1 = m->ros_marker_points_img[(resol*resol)-resol]; + pt2 = m->ros_marker_points_img[(resol*resol)-1]; + + m->ros_corners_3D[0] = cloud(pt1.x, pt1.y); + m->ros_corners_3D[1] = cloud(pt2.x, pt2.y); + m->ros_corners_3D[2] = cloud(pt3.x, pt3.y); + m->ros_corners_3D[3] = cloud(pt4.x, pt4.y); + + if(ori >= 0 && ori < 4){ + if(ori != 0){ + std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end()); + } + } + else + ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id); + + //Get the 3D marker points + BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img) + pixels.push_back(cv::Point(p.x, p.y)); + ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels); + + //Use the kinect data to find a plane and pose for the marker + int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose); + } } } diff --git a/ar_track_alvar/readme_images/MarkerAxes.png b/ar_track_alvar/readme_images/MarkerAxes.png new file mode 100644 index 0000000..5db7196 Binary files /dev/null and b/ar_track_alvar/readme_images/MarkerAxes.png differ diff --git a/ar_track_alvar/readme_images/MarkerData_0.png b/ar_track_alvar/readme_images/MarkerData_0.png new file mode 100644 index 0000000..71304c5 Binary files /dev/null and b/ar_track_alvar/readme_images/MarkerData_0.png differ diff --git a/ar_track_alvar/readme_images/MarkerFlagExample.png b/ar_track_alvar/readme_images/MarkerFlagExample.png new file mode 100644 index 0000000..8a6166b Binary files /dev/null and b/ar_track_alvar/readme_images/MarkerFlagExample.png differ diff --git a/ar_track_alvar/readme_images/MarkerRef.png b/ar_track_alvar/readme_images/MarkerRef.png new file mode 100644 index 0000000..24a4e53 Binary files /dev/null and b/ar_track_alvar/readme_images/MarkerRef.png differ diff --git a/ar_track_alvar/readme_images/colors.png b/ar_track_alvar/readme_images/colors.png new file mode 100644 index 0000000..864780d Binary files /dev/null and b/ar_track_alvar/readme_images/colors.png differ diff --git a/ar_track_alvar/readme_images/corners.png b/ar_track_alvar/readme_images/corners.png new file mode 100644 index 0000000..d423739 Binary files /dev/null and b/ar_track_alvar/readme_images/corners.png differ diff --git a/ar_track_alvar/src/Color.cpp b/ar_track_alvar/src/Color.cpp new file mode 100644 index 0000000..a2296b9 --- /dev/null +++ b/ar_track_alvar/src/Color.cpp @@ -0,0 +1,49 @@ +#include "ar_track_alvar/Color.h" + +namespace col +{ + color_t get_color(char color_name) + { + color_t color = COLOR_BLACK; + switch(color_name) + { + case 'R': case 'r': color = COLOR_RED; break; + case 'B': case 'b': color = COLOR_BLUE; break; + case 'G': case 'g': color = COLOR_GREEN; break; + case 'Y': case 'y': color = COLOR_YELLOW; break; + case 'P': case 'p': color = COLOR_PINK; break; + case 'S': case 's': color = COLOR_SKY; break; + default: color = COLOR_BLACK; break; + } + return color; + } + + unsigned char get_color(color_t p_color) + { + unsigned char result = 0; + if((p_color == COLOR_RED) || (p_color == COLOR_YELLOW) || (p_color == COLOR_PINK)) + result |= 0x04; + if((p_color == COLOR_GREEN) || (p_color == COLOR_YELLOW) || (p_color == COLOR_SKY)) + result |= 0x02; + if((p_color == COLOR_BLUE) || (p_color == COLOR_PINK) || (p_color == COLOR_SKY)) + result |= 0x01; + return result; + } + + void change_color(IplImage *p_img, unsigned char p_color) + { + for( int y=0; yheight; y++ ) + { + uchar* ptr = (uchar*) ( p_img->imageData + y * p_img->widthStep ); + for( int x=0; xwidth; x++ ) + { + if(ptr[3*x+2] == 0) + { + ptr[3*x+2] = ((p_color >> 2)&0x01)*150; //r + ptr[3*x+1] = ((p_color >> 1)&0x01)*150; //g + ptr[3*x+0] = ((p_color >> 0)&0x01)*200; //b + } + } + } + } +} diff --git a/ar_track_alvar/src/ConnectedComponents.cpp b/ar_track_alvar/src/ConnectedComponents.cpp index 1ceaeac..99c652a 100644 --- a/ar_track_alvar/src/ConnectedComponents.cpp +++ b/ar_track_alvar/src/ConnectedComponents.cpp @@ -120,7 +120,7 @@ void LabelingCvSeq::LabelSquares(IplImage* image, bool visualize) CvSeq* result = cvApproxPoly(contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.035, 0 ); // TODO: Parameters? - if( result->total == 4 && CheckBorder(result, image->width, image->height) && + if( result->total == 4 && CheckBorder(result, image->width, image->height) && fabs(cvContourArea(result,CV_WHOLE_SEQ)) > _min_area && // TODO check limits cvCheckContourConvexity(result) ) // ttehop: Changed to 'contours' instead of 'result' { @@ -140,7 +140,7 @@ void LabelingCvSeq::LabelSquares(IplImage* image, bool visualize) blob_corners[i].resize(4); CvSeq* sq = (CvSeq*)cvGetSeqElem(squares, i); CvSeq* square_contour = (CvSeq*)cvGetSeqElem(square_contours, i); - + for(int j = 0; j < 4; ++j) { CvPoint* pt0 = (CvPoint*)cvGetSeqElem(sq, j); @@ -207,7 +207,7 @@ void LabelingCvSeq::LabelSquares(IplImage* image, bool visualize) // 1, cvSize(3,3), cvSize(-1,-1), // cvTermCriteria( // CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,10,1e-4)); - + // TODO: Now there is a wierd systematic 0.5 pixel error that is fixed here... //intc.x += 0.5; //intc.y += 0.5; @@ -264,7 +264,7 @@ CvSeq* LabelingCvSeq::LabelImage(IplImage* image, int min_size, bool approx) cvAdaptiveThreshold(gray, bw, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY_INV, thresh_param1, thresh_param2); CvSeq* contours; - CvSeq* edges = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage); + //CvSeq* edges = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage); //useless CvSeq* squares = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage); cvFindContours(bw, storage, &contours, sizeof(CvContour), @@ -280,7 +280,7 @@ CvSeq* LabelingCvSeq::LabelImage(IplImage* image, int min_size, bool approx) contours = contours->h_next; continue; } - + if(approx) { CvSeq* result = cvApproxPoly(contours, sizeof(CvContour), storage, @@ -290,10 +290,10 @@ CvSeq* LabelingCvSeq::LabelImage(IplImage* image, int min_size, bool approx) { cvSeqPush(squares, result); } - } + } else cvSeqPush(squares, contours); - + contours = contours->h_next; } diff --git a/ar_track_alvar/src/CubeCreator.cpp b/ar_track_alvar/src/CubeCreator.cpp new file mode 100644 index 0000000..91d50ff --- /dev/null +++ b/ar_track_alvar/src/CubeCreator.cpp @@ -0,0 +1,353 @@ +#include "ar_track_alvar/MultiMarker.h" +#include "ar_track_alvar/Color.h" +#include "highgui.h" +using namespace std; +using namespace alvar; + +struct State { + IplImage *img; + stringstream filename; + double minx, miny, maxx, maxy; // top-left and bottom-right in pixel units + MultiMarker multi_marker; + + // General options + bool prompt; + double units; // how many pixels per one unit + double marker_side_len; // marker side len in current units + double cube_side_len; // marker side len in current units + int marker_type; // 0:MarkerData, 1:ArToolkit + int marker_id; + double posx, posy; // The position of marker center in the given units + double transx, transy, transz; + int rot[9]; + double content_res; + double margin_res; + bool array; + unsigned char color; + + // MarkerData specific options + MarkerData::MarkerContentType marker_data_content_type; + + State() + : img(0), + prompt(false), + units(96.0/2.54), // cm assuming 96 dpi + marker_side_len(9.0), // 9 cm + cube_side_len(9.0), //9cm + marker_type(0), + marker_id(0), + posx(0), posy(0), + transx(0), transy(0), transz(0), + content_res(0), // 0 uses default + margin_res(0.0), // 0.0 uses default (can be n*0.5) + array(false), + color(0), + marker_data_content_type(MarkerData::MARKER_CONTENT_TYPE_NUMBER) + {} + ~State() { + if (img) cvReleaseImage(&img); + } + + IplImage *rotateImage2(const IplImage *src, float angleDegrees) + { + // Create a map_matrix, where the left 2x2 matrix + // is the transform and the right 2x1 is the dimensions. + float m[6]; + CvMat M = cvMat(2, 3, CV_32F, m); + int w = src->width; + int h = src->height; + float angleRadians = angleDegrees * ((float)CV_PI / 180.0f); + m[0] = (float)(cos(angleRadians)); + m[1] = (float)(sin(angleRadians)); + m[3] = -m[1]; + m[4] = m[0]; + m[2] = w*0.5f; + m[5] = h*0.5f; + + // Make a spare image for the result + CvSize sizeRotated; + sizeRotated.width = cvRound(h); + sizeRotated.height = cvRound(w); + + // Rotate + IplImage *imageRotated = cvCreateImage(sizeRotated, + src->depth, src->nChannels); + + // Transform the image + cvGetQuadrangleSubPix(src, imageRotated, &M); + + return imageRotated; + } + + void AddMarker(const char *id) { + std::cout<<"ADDING MARKER "< new_maxx) new_maxx = maxx; + if (maxy > new_maxy) new_maxy = maxy; + IplImage *new_img = cvCreateImage(cvSize(int(new_maxx-new_minx+0.5), int(new_maxy-new_miny+0.5)), IPL_DEPTH_8U, 1); + cvSet(new_img, cvScalar(255)); + CvRect roi = cvRect(int(minx-new_minx+0.5), int(miny-new_miny+0.5), img->width, img->height); + cvSetImageROI(new_img, roi); + cvCopy(img, new_img); + cvReleaseImage(&img); + img = new_img; + roi.x = int((posx*units) - (marker_side_len*units/2.0) - new_minx + 0.5); + roi.y = int((posy*units) - (marker_side_len*units/2.0) - new_miny + 0.5); + roi.width = int(marker_side_len*units+0.5); roi.height = int(marker_side_len*units+0.5); + cvSetImageROI(img, roi); + minx = new_minx; miny = new_miny; + maxx = new_maxx; maxy = new_maxy; + } + + int idi = atoi(id); + md.SetContent(marker_data_content_type, idi, 0); + if (filename.str().length()<64) filename<<"_"< 0) st.marker_id=atoi(s.c_str()); + st.AddCube(); + st.Save(); + } + } + + // Output usage message + if (st.prompt) + { + std::string filename(argv[0]); + filename = filename.substr(filename.find_last_of('\\') + 1); + std::cout << "#=============#" << std::endl; + std::cout << "= CubeCreator =" << std::endl; + std::cout << "#=============#" << std::endl; + std::cout << std::endl; + std::cout << "Description:" << std::endl; + std::cout << " This is a utility to generate pattern with markers for to create cubes." << std::endl; + std::cout << " This will generate both the PNG and XML files where you run this utility." << std::endl; + std::cout << " Once the PNG is printed, do not cut all markers independently, but only the entire pattern." << std::endl; + std::cout << std::endl; + std::cout << "Options usage:" << std::endl; + std::cout << " " << filename << " [options] argument" << std::endl; + std::cout << std::endl; + std::cout << " 65535 marker with number 65535" << std::endl; + std::cout << " -u 96 use units corresponding to 1.0 unit per 96 pixels" << std::endl; + std::cout << " -uin use inches as units (assuming 96 dpi)" << std::endl; + std::cout << " -ucm use cm's as units (assuming 96 dpi) " << std::endl; + std::cout << " -sm 5.0 use marker size 5.0x5.0 units (default 9.0x9.0)" << std::endl; + std::cout << " -sc 5.0 use cube size 5.0x5.0 units (default 9.0x9.0)" << std::endl; + std::cout << " -r 5 marker content resolution -- 0 uses default" << std::endl; + std::cout << " -m 1.0 marker margin resolution -- 0 uses default" << std::endl; + std::cout << " -p prompt marker placements interactively from the user" << std::endl; + std::cout << " -c B cube color : B:blue R:red G:green P:pink S:sky Y:yellow -- default black" << std::endl; + std::cout << std::endl; + + // Interactive stuff here + st.marker_type = 0; + st.marker_data_content_type = MarkerData::MARKER_CONTENT_TYPE_NUMBER; + std::cout<<"\nPrompt marker placements interactively"< 0) st.marker_id=atoi(s.c_str()); + if (st.marker_id < 0) break; + + std::cout<<" Marker size (cm): "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() > 0) st.marker_side_len = atof(s.c_str()); + + st.cube_side_len = st.marker_side_len; + std::cout<<" Cube size (cm): "; std::flush(std::cout); + std::getline(std::cin, s); + if (s.length() > 0) + st.cube_side_len = atof(s.c_str()); + + std::cout<<" Colors :" << std::endl; + std::cout<<" B - blue" << std::endl; + std::cout<<" R - red" << std::endl; + std::cout<<" G - green" << std::endl; + std::cout<<" P - pink" << std::endl; + std::cout<<" S - sky" << std::endl; + std::cout<<" Y - yellow" << std::endl; + std::cout<<" default - black :" << std::endl; + std::cout<<" Marker color : "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() <= 0) s = " "; + col::color_t tmp_color = col::get_color(s[0]); + st.color = col::get_color(tmp_color); + + st.AddCube(); + st.Save(); + st.multi_marker = MultiMarker(); + } + } + + return 0; + } + catch (const std::exception &e) { + std::cout << "Exception: " << e.what() << endl; + } + catch (...) { + std::cout << "Exception: unknown" << std::endl; + } +} diff --git a/ar_track_alvar/src/CvTestbed.cpp b/ar_track_alvar/src/CvTestbed.cpp index 0a91986..7392d06 100644 --- a/ar_track_alvar/src/CvTestbed.cpp +++ b/ar_track_alvar/src/CvTestbed.cpp @@ -125,7 +125,7 @@ bool CvTestbed::StartVideo(Capture *_cap, const char *_wintitle) { size_t CvTestbed::SetImage(const char *title, IplImage *ipl, bool release_at_exit /* =false */) { size_t index = GetImageIndex(title); - if (index == -1) { + if (index == (size_t)-1) { // If the title wasn't found create new Image i(ipl, title, false, release_at_exit); images.push_back(i); diff --git a/ar_track_alvar/src/DeportedMarkerCreator.cpp b/ar_track_alvar/src/DeportedMarkerCreator.cpp new file mode 100644 index 0000000..89223b9 --- /dev/null +++ b/ar_track_alvar/src/DeportedMarkerCreator.cpp @@ -0,0 +1,265 @@ +#include "ar_track_alvar/MultiMarker.h" +#include "ar_track_alvar/Color.h" +#include "highgui.h" +using namespace std; +using namespace alvar; + +struct State { + IplImage *img; + stringstream filename; + double minx, miny, maxx, maxy; // top-left and bottom-right in pixel units + MultiMarker multi_marker; + + // General options + bool prompt; + double units; // how many pixels per one unit + double marker_side_len; // marker side len in current units + int marker_type; // 0:MarkerData, 1:ArToolkit + double posx, posy, posz; // The position of marker center in the given units + int content_res; + double margin_res; + bool array; + unsigned char color; + + // MarkerData specific options + MarkerData::MarkerContentType marker_data_content_type; + + State() + : img(0), + filename("marker"), + minx(0), miny(0), maxx(0), maxy(0), + prompt(false), + units(96.0/2.54), // cm assuming 96 dpi + marker_side_len(9.0), // 9 cm + marker_type(0), + posx(0), posy(0), posz(0), + content_res(0), // 0 uses default + margin_res(0.0), // 0.0 uses default (can be n*0.5) + array(false), + color(0), + marker_data_content_type(MarkerData::MARKER_CONTENT_TYPE_NUMBER) + {} + ~State() { + if (img) cvReleaseImage(&img); + } + + void AddMarker(const char *id) { + std::cout<<"ADDING MARKER "< 8) break; + } + } + + md.ScaleMarkerToImage(img); + cvResetImageROI(img); + IplImage* color_img = cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3); + cvCvtColor(img, color_img, CV_GRAY2RGB); + col::change_color(color_img, color); + img = color_img; + } + + void Save() { + if (img) { + std::stringstream filenamexml; + filenamexml< positive side" << std::endl; + std::cout << "# #" << std::endl; + std::cout << "# #" << std::endl; + std::cout << "#######" << std::endl; + std::cout << std::endl; +} + +int main(int argc, char *argv[]) +{ + State st; + try { + if (argc < 2) st.prompt = true; + for (int i=1; i" << std::endl; + std::cout << " -s 5.0 use marker size 5.0x5.0 units (default 9.0x9.0)" << std::endl; + std::cout << " -r 5 marker content resolution -- 0 uses default" << std::endl; + std::cout << " -m 2.0 marker margin resolution -- 0 uses default" << std::endl; + std::cout << " -p prompt marker placements interactively from the user" << std::endl; + std::cout << " -c B marker color : B:blue R:red G:green P:pink S:sky Y:yellow -- default black" << std::endl; + std::cout << std::endl; + + // Interactive stuff here + st.marker_type = 0; + st.marker_data_content_type = MarkerData::MARKER_CONTENT_TYPE_NUMBER; + std::cout<<"\nPrompt marker placements interactively"< 0) marker_id=atoi(s.c_str()); + if (marker_id < 0) break; + + std::cout<<" Marker size (cm): "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() > 0) st.marker_side_len = atof(s.c_str()); + + print_tag(); + std::cout<<" Marker side translation : "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() > 0) posx=atof(s.c_str()); + std::cout<<" Marker height translation : "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() > 0) posy=atof(s.c_str()); + std::cout<<" Marker depth translation : "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() > 0) posz=atof(s.c_str()); + st.posx=-posx; + st.posy=posy; + st.posz=-posz; + std::cout<<" Colors :" << std::endl; + std::cout<<" B - blue" << std::endl; + std::cout<<" R - red" << std::endl; + std::cout<<" G - green" << std::endl; + std::cout<<" P - pink" << std::endl; + std::cout<<" S - sky" << std::endl; + std::cout<<" Y - yellow" << std::endl; + std::cout<<" default - black :" << std::endl; + std::cout<<" Marker color : "; std::flush(std::cout); + std::getline(std::cin, s); if (s.length() <= 0) s = " "; + col::color_t tmp_color = col::get_color(s[0]); + st.color = col::get_color(tmp_color); + + ss<(par+ellipse.size.width/2), static_cast(par+ellipse.size.height/2)), -ellipse.angle, 0, 360, color, type); } -void BuildHideTexture(IplImage *image, IplImage *hide_texture, - Camera *cam, double gl_modelview[16], - PointDouble topleft, PointDouble botright) +void BuildHideTexture(IplImage *image, IplImage *hide_texture, + Camera *cam, double gl_modelview[16], + PointDouble topleft, PointDouble botright) { assert(image->origin == 0); // Currently only top-left origin supported double kx=1.0; double ky=1.0; - + double width = abs(botright.x - topleft.x); double height = abs(botright.y - topleft.y); - + //GLint vp[4]; //viewport //GLdouble winx[8]; // point's coordinates in windowcoordinates - //GLdouble winy[8]; + //GLdouble winy[8]; //GLdouble winz[8]; double objx; double objy; //GLdouble objz; unsigned char pixels[8][3]; - unsigned char color[3]={0,0,0}; - int i=0,j=0,t=0; + int i=0,j=0; double ox,oy,ya,yb,xc,xd,offset; double sizex = width/4, size2x=width/2; double sizey = height/4, size2y=height/2; @@ -123,9 +122,9 @@ void BuildHideTexture(IplImage *image, IplImage *hide_texture, objy = height/2*ky; //cout<width<<","<height<height;j++){ @@ -148,13 +147,13 @@ void BuildHideTexture(IplImage *image, IplImage *hide_texture, offset = fmod((objy-oy), size2y); if( offset < sizey) ya = objy + offset; - else - ya = objy+size2y-offset; + else + ya = objy+size2y-offset; offset = fmod((oy+objy), size2y); if( offset < sizey) yb = -objy - offset; - else - yb = -objy-size2y+offset; + else + yb = -objy-size2y+offset; s=(oy+objy); double points3d[4][3] = { @@ -198,8 +197,8 @@ void BuildHideTexture(IplImage *image, IplImage *hide_texture, else if ((i<4*w)|(j<4*w)|(i>hide_texture->width-4*w)|(j>hide_texture->width-4*w)) opaque=200; else - opaque=255; - + opaque=255; + cvSet2D(hide_texture, j, i, cvScalar( (((lr-r)*pixels[7][0] + r*pixels[6][0]+ s* pixels[4][0] + (ls-s)* pixels[5][0])/l2r), (((lr-r)*pixels[7][1] + r*pixels[6][1]+ s* pixels[4][1] + (ls-s)* pixels[5][1])/l2r), @@ -210,16 +209,16 @@ void BuildHideTexture(IplImage *image, IplImage *hide_texture, } } -void DrawTexture(IplImage *image, IplImage *texture, - Camera *cam, double gl_modelview[16], - PointDouble topleft, PointDouble botright) +void DrawTexture(IplImage *image, IplImage *texture, + Camera *cam, double gl_modelview[16], + PointDouble topleft, PointDouble botright) { assert(image->origin == 0); // Currently only top-left origin supported double width = abs(botright.x - topleft.x); double height = abs(botright.y - topleft.y); double objx = width/2; double objy = height/2; - + // Project corners double points3d[4][3] = { -objx, -objy, 0, @@ -232,7 +231,7 @@ void DrawTexture(IplImage *image, IplImage *texture, cvInitMatHeader(&points3d_mat, 4, 3, CV_64F, points3d); cvInitMatHeader(&points2d_mat, 4, 2, CV_64F, points2d); cam->ProjectPoints(&points3d_mat, gl_modelview, &points2d_mat); - + // Warp texture and mask using the perspective that is based on the corners double map[9]; CvMat map_mat = cvMat(3, 3, CV_64F, map); @@ -267,7 +266,7 @@ void DrawTexture(IplImage *image, IplImage *texture, } cvWarpPerspective(img, img2, &map_mat); cvWarpPerspective(mask, mask2, &map_mat, 0); - + cvCopy(img2, image, mask2); cvReleaseImage(&img); diff --git a/ar_track_alvar/src/MultiMarker.cpp b/ar_track_alvar/src/MultiMarker.cpp index 9aa389f..b10f542 100644 --- a/ar_track_alvar/src/MultiMarker.cpp +++ b/ar_track_alvar/src/MultiMarker.cpp @@ -105,7 +105,7 @@ bool MultiMarker::SaveText(const char* fname) { { CvPoint3D64f X = pointcloud[pointcloud_index(marker_indices[i], j)]; file_op< tmp_pointcloud = pointcloud; + for(int src = 0; src < 3; src++) + for(int dest = 0; dest < 3; dest++) + { + int weight = rot[src*3 + dest]; + if(weight != 0) + { + double tmp[4]; + for(size_t j = 0; j < 4; ++j) + { + if(src == 0) + tmp[j] = pointcloud[pointcloud_index(marker_id, j)].x; + else if(src == 1) + tmp[j] = pointcloud[pointcloud_index(marker_id, j)].y; + else if(src == 2) + tmp[j] = pointcloud[pointcloud_index(marker_id, j)].z; + } + for(size_t j = 0; j < 4; ++j) + { + if(dest == 0) + tmp_pointcloud[pointcloud_index(marker_id, j)].x = weight*tmp[j]; + else if(dest == 1) + tmp_pointcloud[pointcloud_index(marker_id, j)].y = weight*tmp[j]; + else if(dest == 2) + tmp_pointcloud[pointcloud_index(marker_id, j)].z = weight*tmp[j]; + } + } + } + + pointcloud = tmp_pointcloud; +} + void MultiMarker::PointCloudCopy(const MultiMarker *m) { pointcloud.clear(); pointcloud = m->pointcloud; // TODO: Is this copy operation ok? diff --git a/ar_track_alvar/src/Optimization.cpp b/ar_track_alvar/src/Optimization.cpp index ea60c2a..9760ea2 100644 --- a/ar_track_alvar/src/Optimization.cpp +++ b/ar_track_alvar/src/Optimization.cpp @@ -37,7 +37,7 @@ Optimization::Optimization(int n_params, int n_meas) J = cvCreateMat(n_meas, n_params, CV_64F); cvZero(J); JtJ = cvCreateMat(n_params, n_params, CV_64F); cvZero(JtJ); tmp = cvCreateMat(n_params, n_meas, CV_64F); cvZero(tmp); - W = cvCreateMat(n_meas, n_meas, CV_64F); cvZero(W); + W = cvCreateMat(n_meas, n_meas, CV_64F); cvZero(W); diag = cvCreateMat(n_params, n_params, CV_64F); cvZero(diag); err = cvCreateMat(n_meas, 1, CV_64F); cvZero(err); delta = cvCreateMat(n_params, 1, CV_64F); cvZero(delta); @@ -77,19 +77,18 @@ double Optimization::CalcTukeyWeight(double residual, double c) } else ret = (c*c)/6.0; - + if(residual) ret = fabs(sqrt(ret)/residual); else ret = 1.0; // ??? - + return ret; } double Optimization::CalcTukeyWeightSimple(double residual, double c) { //const double c = 3; - double ret=0; double x2 = residual*residual; if(x2rows; - int n_meas = measurements->rows; double error_new = 0; double error_old = 0; double n1, n2; @@ -182,7 +179,7 @@ double Optimization::Optimize(CvMat* parameters, // Initial values are set if( ((n1/n2) < stop) || (cntr >= max_iter) ) - goto end; + goto end; break; @@ -208,10 +205,10 @@ double Optimization::Optimize(CvMat* parameters, // Initial values are set cvAdd(JtJ, diag, JtJ); cvInv(JtJ, JtJ, CV_SVD); cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T); - + if(weights) cvGEMM(tmp, W, 1, 0, 0, tmp, 0); - + cvMatMul(tmp, err, delta); cvAdd(delta, parameters, tmp_par); @@ -219,7 +216,7 @@ double Optimization::Optimize(CvMat* parameters, // Initial values are set cvSub(measurements, x_tmp1, err); error_new = cvNorm(err, 0, CV_L2); - + if(error_new < error_old) { cvCopy(tmp_par, parameters); @@ -244,7 +241,7 @@ double Optimization::Optimize(CvMat* parameters, // Initial values are set break; case (TUKEY_LM) : - + cvSetIdentity(diag, cvRealScalar(lambda)); // Tukey weights @@ -272,7 +269,7 @@ double Optimization::Optimize(CvMat* parameters, // Initial values are set cvSub(measurements, x_tmp1, err); error_new = cvNorm(err, 0, CV_L2); - + if(error_new < error_old) { cvCopy(tmp_par, parameters); @@ -293,7 +290,7 @@ double Optimization::Optimize(CvMat* parameters, // Initial values are set { goto end; } - + break; } ++cntr; diff --git a/ar_track_alvar/src/SampleMarkerCreator.cpp b/ar_track_alvar/src/SampleMarkerCreator.cpp index c0a8f04..6f60d61 100644 --- a/ar_track_alvar/src/SampleMarkerCreator.cpp +++ b/ar_track_alvar/src/SampleMarkerCreator.cpp @@ -23,7 +23,7 @@ struct State { MarkerData::MarkerContentType marker_data_content_type; bool marker_data_force_strong_hamming; - State() + State() : img(0), prompt(false), units(96.0/2.54), // cm assuming 96 dpi @@ -67,8 +67,8 @@ struct State { cvCopy(img, new_img); cvReleaseImage(&img); img = new_img; - roi.x = int((posx*units) - (marker_side_len*units/2.0) - new_minx + 0.5); - roi.y = int((posy*units) - (marker_side_len*units/2.0) - new_miny + 0.5); + roi.x = int((posx*units) - (marker_side_len*units/2.0) - new_minx + 0.5); + roi.y = int((posy*units) - (marker_side_len*units/2.0) - new_miny + 0.5); roi.width = int(marker_side_len*units+0.5); roi.height = int(marker_side_len*units+0.5); cvSetImageROI(img, roi); minx = new_minx; miny = new_miny; @@ -131,13 +131,13 @@ int main(int argc, char *argv[]) try { if (argc < 2) st.prompt = true; for (int i=1; i