From 2f1581371fd1db09851c2334e7cbd84c0a8e8c34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Thu, 18 Jul 2019 15:00:52 +0000 Subject: [PATCH 01/22] FindMarkerBundlesNoKinect: implement params and dynamic reconfigure --- .../launch/pr2_bundle_no_kinect.launch | 11 +- .../nodes/FindMarkerBundlesNoKinect.cpp | 211 +++++++++++++----- .../nodes/IndividualMarkersNoKinect.cpp | 2 +- 3 files changed, 168 insertions(+), 56 deletions(-) diff --git a/ar_track_alvar/launch/pr2_bundle_no_kinect.launch b/ar_track_alvar/launch/pr2_bundle_no_kinect.launch index c7892dd..a7f4950 100755 --- a/ar_track_alvar/launch/pr2_bundle_no_kinect.launch +++ b/ar_track_alvar/launch/pr2_bundle_no_kinect.launch @@ -9,5 +9,14 @@ - + + + + + + + + + + diff --git a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp index 013588b..bf8b560 100644 --- a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp @@ -1,13 +1,10 @@ /* Software License Agreement (BSD License) - Copyright (c) 2012, Scott Niekum All rights reserved. - Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above @@ -17,7 +14,6 @@ * Neither the name of the Willow Garage nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -30,7 +26,6 @@ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - author: Scott Niekum */ @@ -45,7 +40,10 @@ #include #include #include +#include #include +#include +#include using namespace alvar; using namespace std; @@ -67,16 +65,21 @@ 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; +bool enable_switched = false; +bool enabled = 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; +double max_frequency = 8.0; // default maximum frequency +int marker_resolution = 5; // default marker resolution +int marker_margin = 2; // default marker margin +int n_bundles = 0; void GetMultiMarkerPoses(IplImage *image); void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg); @@ -89,7 +92,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 +103,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 +176,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 +223,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; idata; + enabled = msg->data; +} int main(int argc, char *argv[]) { ros::init (argc, argv, "marker_detect"); - ros::NodeHandle n; - - if(argc < 8){ - std::cout << std::endl; - cout << "Not enough arguments provided." << endl; - cout << "Usage: ./findMarkerBundles " << endl; - std::cout << std::endl; - return 0; + ros::NodeHandle n, pn("~"); + + vector bundle_files; + + if(argc > 1) { + ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings."); + + if(argc < 8){ + std::cout << std::endl; + cout << "Not enough arguments provided." << endl; + cout << "Usage: ./findMarkerBundles " + << " "; + std::cout << std::endl; + return 0; + } + + // Get params from command line + marker_size = atof(argv[1]); + max_new_marker_error = atof(argv[2]); + max_track_error = atof(argv[3]); + cam_image_topic = argv[4]; + cam_info_topic = argv[5]; + output_frame = argv[6]; + int n_args_before_list = 7; + n_bundles = argc - n_args_before_list; + for (int i = 0; i < n_bundles; ++i) { + bundle_files.push_back(string(argv[i + n_args_before_list])); + } + + } else { + std::string bundle_string; + // Get params from ros param server. + pn.param("marker_size", marker_size, 10.0); + pn.param("max_new_marker_error", max_new_marker_error, 0.08); + pn.param("max_track_error", max_track_error, 0.2); + pn.param("max_frequency", max_frequency, 8.0); + pn.setParam("max_frequency", max_frequency); // in case it was not set. + pn.param("marker_resolution", marker_resolution, 5); + pn.param("marker_margin", marker_margin, 2); + pn.param("bundle_files", bundle_string, ""); + if (!pn.getParam("output_frame", output_frame)) { + ROS_ERROR("Param 'output_frame' has to be set."); + exit(EXIT_FAILURE); + } + + // extract bundles + std::stringstream ss(bundle_string); + std::string token; + while (std::getline(ss, token, ' ')) { + if (!token.empty()) { + bundle_files.push_back(token); + } + } + n_bundles = bundle_files.size(); + + // Camera input topics. Use remapping to map to your camera topics. + cam_image_topic = "camera_image"; + cam_info_topic = "camera_info"; } - // Get params from command line - marker_size = atof(argv[1]); - max_new_marker_error = atof(argv[2]); - max_track_error = atof(argv[3]); - cam_image_topic = argv[4]; - cam_info_topic = argv[5]; - output_frame = argv[6]; - int n_args_before_list = 7; - n_bundles = argc - n_args_before_list; - - marker_detector.SetMarkerSize(marker_size); - multi_marker_bundles = new MultiMarkerBundle*[n_bundles]; + // Set dynamically configurable parameters so they don't get replaced by default values + pn.setParam("marker_size", marker_size); + pn.setParam("max_new_marker_error", max_new_marker_error); + pn.setParam("max_track_error", max_track_error); + + marker_detector.SetMarkerSize(marker_size, marker_resolution, marker_margin); + multi_marker_bundles = new MultiMarkerBundle*[n_bundles]; bundlePoses = new Pose[n_bundles]; - master_id = new int[n_bundles]; - bundle_indices = new std::vector[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]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML); + multi_marker_bundles[i] = new MultiMarkerBundle(id_vector); + multi_marker_bundles[i]->Load(bundle_files[i].c_str(), 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; + else { + cout<<"Cannot load file "<< bundle_files[i] << endl; return 0; - } - } + } + } // Set up camera, listeners, and broadcasters cam = new Camera(n, cam_info_topic); @@ -329,17 +395,54 @@ 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); - + + // Prepare dynamic reconfiguration + dynamic_reconfigure::Server < ar_track_alvar::ParamsConfig > server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&configCallback, _1, _2); + server.setCallback(f); + //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); - cam_sub_ = it_.subscribe (cam_image_topic, 1, &getCapCallback); - ros::spin(); + // Run at the configured rate, discarding pointcloud msgs if necessary + ros::Rate rate(max_frequency); + + /// Subscriber for enable-topic so that a user can turn off the detection if it is not used without + /// having to use the reconfigure where he has to know all parameters + ros::Subscriber enable_sub_ = pn.subscribe("enable_detection", 1, &enableCallback); + + enable_switched = true; + while (ros::ok()) + { + ros::spinOnce(); + rate.sleep(); + + if (std::abs((rate.expectedCycleTime() - ros::Duration(1.0 / max_frequency)).toSec()) > 0.001) + { + // Change rate dynamically; if must be above 0, as 0 will provoke a segfault on next spinOnce + ROS_DEBUG("Changing frequency from %.2f to %.2f", 1.0 / rate.expectedCycleTime().toSec(), max_frequency); + rate = ros::Rate(max_frequency); + } + + if (enable_switched) + { + // Enable/disable switch: subscribe/unsubscribe to make use of pointcloud processing nodelet + // lazy publishing policy; in CPU-scarce computer as TurtleBot's laptop this is a huge saving + if (enabled) { + ROS_INFO ("Subscribing to image topic"); + cam_sub_ = it_.subscribe(cam_image_topic, 1, &getCapCallback); + } + else + cam_sub_.shutdown(); + enable_switched = false; + } + } return 0; } diff --git a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp index d5a439e..c2cbf86 100644 --- a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp +++ b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp @@ -65,7 +65,7 @@ MarkerDetector marker_detector; bool enableSwitched = false; bool enabled = true; -double max_frequency; +double max_frequency = 8.0; double marker_size; double max_new_marker_error; double max_track_error; From bee4646e9aa3c8c740ae4e7521feaa2d8ca5c15a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Sat, 25 Jan 2020 20:43:30 +0000 Subject: [PATCH 02/22] publish transform in output frame --- .../nodes/FindMarkerBundlesNoKinect.cpp | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp index bf8b560..21403a6 100644 --- a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp @@ -124,17 +124,6 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin); tf::Transform markerPose = t * m; - //Publish the cam to marker transform for main marker in each bundle - if(type==MAIN_MARKER){ - std::string markerFrame = "ar_marker_"; - std::stringstream out; - out << id; - std::string id_string = out.str(); - markerFrame += id_string; - tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); - tf_broadcaster->sendTransform(camToMarker); - } - //Create the rviz visualization message tf::poseTFToMsg (markerPose, rvizMarker->pose); rvizMarker->header.frame_id = image_msg->header.frame_id; @@ -186,6 +175,15 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ ar_pose_marker->header.frame_id = output_frame; ar_pose_marker->header.stamp = image_msg->header.stamp; ar_pose_marker->id = id; + + //Publish the output frame to marker transform for main marker in each bundle + std::string markerFrame = "ar_marker_"; + std::stringstream out; + out << id; + std::string id_string = out.str(); + markerFrame += id_string; + tf::StampedTransform outputToMarker (tagPoseOutput, image_msg->header.stamp, output_frame, markerFrame.c_str()); + tf_broadcaster->sendTransform(outputToMarker); } else ar_pose_marker = NULL; From 0274618da3e165e6dc62c86a6211c97360568d1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Sat, 8 Feb 2020 13:27:32 +0000 Subject: [PATCH 03/22] FindMarkerBundles: clear errors reported by linter --- .../nodes/FindMarkerBundlesNoKinect.cpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp index 21403a6..eb4504b 100644 --- a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp @@ -61,7 +61,7 @@ ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_; tf::TransformListener *tf_listener; tf::TransformBroadcaster *tf_broadcaster; MarkerDetector marker_detector; -MultiMarkerBundle **multi_marker_bundles=NULL; +MultiMarkerBundle **multi_marker_bundles=nullptr; Pose *bundlePoses; int *master_id; bool *bundles_seen; @@ -83,7 +83,7 @@ int n_bundles = 0; void GetMultiMarkerPoses(IplImage *image); void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg); -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); +void makeMarkerMsgs(int type, int id, Pose &p, const sensor_msgs::ImageConstPtr& image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker); // Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position @@ -104,7 +104,7 @@ void GetMultiMarkerPoses(IplImage *image) { // 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){ +void makeMarkerMsgs(int type, int id, Pose &p, const 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; @@ -182,11 +182,9 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ out << id; std::string id_string = out.str(); markerFrame += id_string; - tf::StampedTransform outputToMarker (tagPoseOutput, image_msg->header.stamp, output_frame, markerFrame.c_str()); + tf::StampedTransform outputToMarker (tagPoseOutput, image_msg->header.stamp, output_frame, markerFrame); tf_broadcaster->sendTransform(outputToMarker); } - else - ar_pose_marker = NULL; } @@ -202,7 +200,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, ros::Duration(1.0)); tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, CamToOutput); } - catch (tf::TransformException ex){ + catch (const tf::TransformException &ex){ ROS_ERROR("%s",ex.what()); } @@ -226,16 +224,16 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) for(int i=0; isize(); i++) + for (auto & marker : *marker_detector.markers) { - int id = (*(marker_detector.markers))[i].GetId(); + int id = static_cast(marker.GetId()); // Draw if id is valid if(id >= 0){ //Mark the bundle that marker belongs to as "seen" for(int j=0; j Date: Sat, 8 Feb 2020 13:34:23 +0000 Subject: [PATCH 04/22] FindMarkerBundles: clear errors reported by linter --- ar_track_alvar/nodes/FindMarkerBundles.cpp | 234 ++++++++++----------- 1 file changed, 116 insertions(+), 118 deletions(-) diff --git a/ar_track_alvar/nodes/FindMarkerBundles.cpp b/ar_track_alvar/nodes/FindMarkerBundles.cpp index e7f83e0..c1bc525 100644 --- a/ar_track_alvar/nodes/FindMarkerBundles.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundles.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -93,13 +94,13 @@ ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_; tf::TransformListener *tf_listener; tf::TransformBroadcaster *tf_broadcaster; MarkerDetector marker_detector; -MultiMarkerBundle **multi_marker_bundles=NULL; +MultiMarkerBundle **multi_marker_bundles=nullptr; 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,28 +108,28 @@ 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) +void draw3dPoints(const ARCloud::Ptr& cloud, const string &frame, int color, int id, double rad) { 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,38 +148,38 @@ 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++){ - p.x = cloud->points[i].x; - p.y = cloud->points[i].y; - p.z = cloud->points[i].z; + for(auto & point : cloud->points){ + p.x = point.x; + p.y = point.y; + p.z = point.z; rvizMarker.points.push_back(p); } - + rvizMarker.lifetime = ros::Duration (1.0); rvizMarkerPub2_.publish (rvizMarker); } -void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int id) +void drawArrow(const gm::Point &start, const tf::Matrix3x3 &mat, const 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(); + rvizMarker.points.clear(); rvizMarker.points.push_back(start); gm::Point end; end.x = start.x + mat[0][i]; @@ -214,7 +215,7 @@ 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); @@ -225,19 +226,18 @@ int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_ } // Reset the marker_status to 1 for all markers in point_cloud for tracking purposes - for (size_t i=0; i 0) master.marker_status[i]=1; + for (int & marker_status : master.marker_status) { + if (marker_status > 0) marker_status=1; } int n_est = 0; // For every detected marker - for (size_t i=0; isize(); i++) + for (auto & i : *marker_detector.markers) { - const Marker* marker = &((*marker_detector.markers)[i]); - int id = marker->GetId(); + const Marker* marker = &i; + int id = static_cast(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,15 +256,15 @@ 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){ + catch (const tf::TransformException &ex){ ROS_ERROR("ERROR InferCorners: %s",ex.what()); return -1; } @@ -276,13 +276,13 @@ 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++){ - bund_corners[i].x /= n_est; - bund_corners[i].y /= n_est; - bund_corners[i].z /= n_est; + bund_corners[i].x /= static_cast(n_est); + bund_corners[i].y /= static_cast(n_est); + bund_corners[i].z /= static_cast(n_est); } } @@ -290,7 +290,7 @@ int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_ } -int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr selected_points, const ARCloud &cloud, Pose &p){ +int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, const ARCloud::Ptr& selected_points, const ARCloud &cloud, Pose &p){ ata::PlaneFitResult res = ata::fitPlane(selected_points); gm::PoseStamped pose; @@ -299,13 +299,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 +313,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 +364,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,19 +378,19 @@ 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++) { vector > pixels; Marker *m = &((*marker_detector.markers)[i]); - int id = m->GetId(); + int id = static_cast(m->GetId()); //cout << "******* ID: " << id << endl; - + //Get the 3D points of the outer corners /* PointDouble corner0 = m->marker_corners_img[0]; @@ -402,22 +402,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 +430,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.emplace_back(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 +463,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 +483,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 -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){ +// Given the pose of a marker, builds the appropriate ROS messages for later publishing +void makeMarkerMsgs(int type, int id, Pose &p, const 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; @@ -527,7 +527,7 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ out << id; std::string id_string = out.str(); markerFrame += id_string; - tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); + tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame); tf_broadcaster->sendTransform(camToMarker); //Create the rviz visualization message @@ -571,7 +571,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; @@ -583,8 +583,6 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ ar_pose_marker->id = id; ar_pose_marker->confidence = confidence; } - else - ar_pose_marker = NULL; } @@ -603,7 +601,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) tf_listener->waitForTransform(output_frame, msg->header.frame_id, msg->header.stamp, ros::Duration(1.0)); tf_listener->lookupTransform(output_frame, msg->header.frame_id, msg->header.stamp, CamToOutput); } - catch (tf::TransformException ex){ + catch (const tf::TransformException &ex){ ROS_ERROR("%s",ex.what()); } @@ -612,7 +610,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 +618,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); @@ -632,9 +630,9 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) IplImage ipl_image = cv_ptr_->image; GetMultiMarkerPoses(&ipl_image, cloud); - for (size_t i=0; isize(); i++) + for (auto & marker : *marker_detector.markers) { - int id = (*(marker_detector.markers))[i].GetId(); + int id = static_cast(marker.GetId()); // Draw if id is valid if(id >= 0){ @@ -644,14 +642,14 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) for(int j=0; j eig_quat(eig_m); - + // Translate back to bullet tfScalar ex = eig_quat.x(); tfScalar ey = eig_quat.y(); @@ -711,15 +709,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 +728,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 +826,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); From 69e143a02fb51928c4144c74b70640683e39b4d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Sat, 8 Feb 2020 15:16:57 +0000 Subject: [PATCH 05/22] FindMarkerBundlesNoKinect: restructure --- ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp index eb4504b..b6a92c4 100644 --- a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp @@ -30,7 +30,6 @@ */ -#include "ar_track_alvar/CvTestbed.h" #include "ar_track_alvar/MarkerDetector.h" #include "ar_track_alvar/MultiMarkerBundle.h" #include "ar_track_alvar/MultiMarkerInitializer.h" @@ -333,7 +332,6 @@ int main(int argc, char *argv[]) pn.param("max_new_marker_error", max_new_marker_error, 0.08); pn.param("max_track_error", max_track_error, 0.2); pn.param("max_frequency", max_frequency, 8.0); - pn.setParam("max_frequency", max_frequency); // in case it was not set. pn.param("marker_resolution", marker_resolution, 5); pn.param("marker_margin", marker_margin, 2); pn.param("bundle_files", bundle_string, ""); @@ -358,6 +356,7 @@ int main(int argc, char *argv[]) } // Set dynamically configurable parameters so they don't get replaced by default values + pn.setParam("max_frequency", max_frequency); pn.setParam("marker_size", marker_size); pn.setParam("max_new_marker_error", max_new_marker_error); pn.setParam("max_track_error", max_track_error); From 43548acc1af23183b6bbc9fa5107da4f025d7417 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Sat, 8 Feb 2020 15:18:03 +0000 Subject: [PATCH 06/22] FindMarkerBundles: port features from FindMarkerBundlesNoKinect --- ar_track_alvar/nodes/FindMarkerBundles.cpp | 178 ++++++++++++++++----- 1 file changed, 137 insertions(+), 41 deletions(-) diff --git a/ar_track_alvar/nodes/FindMarkerBundles.cpp b/ar_track_alvar/nodes/FindMarkerBundles.cpp index c1bc525..c63a8cb 100644 --- a/ar_track_alvar/nodes/FindMarkerBundles.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundles.cpp @@ -33,40 +33,32 @@ author: Scott Niekum */ -#include "ar_track_alvar/CvTestbed.h" #include "ar_track_alvar/MarkerDetector.h" #include "ar_track_alvar/MultiMarkerBundle.h" #include "ar_track_alvar/MultiMarkerInitializer.h" -#include "ar_track_alvar/Shared.h" #include #include #include #include #include +#include #include #include #include -#include -#include #include #include #include -#include -#include -#include -#include #include -#include -#include -#include #include #include #include #include #include +#include +#include #define MAIN_MARKER 1 @@ -105,12 +97,17 @@ bool init = true; ata::MedianFilter **med_filts; int med_filt_size; +bool enable_switched = false; +bool enabled = 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 output_frame; +double max_frequency = 8.0; // default maximum frequency +int marker_resolution = 5; // default marker resolution +int marker_margin = 2; // default marker margin int n_bundles = 0; //Debugging utility function @@ -763,32 +760,97 @@ int calcAndSaveMasterCoords(MultiMarkerBundle &master) return 0; } +void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level) +{ + (void)level; + ROS_INFO("AR tracker reconfigured: %s %.2f %.2f %.2f %.2f", config.enabled ? "ENABLED" : "DISABLED", + config.max_frequency, config.marker_size, config.max_new_marker_error, config.max_track_error); + + enable_switched = enabled != config.enabled; + + enabled = config.enabled; + max_frequency = config.max_frequency; + marker_size = config.marker_size; + max_new_marker_error = config.max_new_marker_error; + max_track_error = config.max_track_error; +} + +void enableCallback(const std_msgs::BoolConstPtr& msg) +{ + enable_switched = enabled != msg->data; + enabled = msg->data; +} int main(int argc, char *argv[]) { - ros::init (argc, argv, "marker_detect"); - ros::NodeHandle n; - - if(argc < 9){ - std::cout << std::endl; - cout << "Not enough arguments provided." << endl; - cout << "Usage: ./findMarkerBundles " << endl; - std::cout << std::endl; - return 0; + ros::init(argc, argv, "marker_detect"); + ros::NodeHandle n, pn("~"); + + vector bundle_files; + + if (argc > 1) { + ROS_WARN( + "Command line arguments are deprecated. Consider using ROS parameters and remappings."); + + if (argc < 9) { + std::cout << std::endl; + cout << "Not enough arguments provided." << endl; + cout + << "Usage: ./findMarkerBundles " + << endl; + std::cout << std::endl; + return 0; + } + + // Get params from command line + marker_size = atof(argv[1]); + max_new_marker_error = atof(argv[2]); + max_track_error = atof(argv[3]); + cam_image_topic = argv[4]; + cam_info_topic = argv[5]; + output_frame = argv[6]; + med_filt_size = atoi(argv[7]); + int n_args_before_list = 8; + n_bundles = argc - n_args_before_list; + + } else { + std::string bundle_string; + // Get params from ros param server. + pn.param("marker_size", marker_size, 10.0); + pn.param("max_new_marker_error", max_new_marker_error, 0.08); + pn.param("max_track_error", max_track_error, 0.2); + pn.param("max_frequency", max_frequency, 8.0); + pn.param("marker_resolution", marker_resolution, 5); + pn.param("marker_margin", marker_margin, 2); + pn.param("med_filt_size", med_filt_size, 10); + pn.param("bundle_files", bundle_string, ""); + if (!pn.getParam("output_frame", output_frame)) { + ROS_ERROR("Param 'output_frame' has to be set."); + exit(EXIT_FAILURE); + } + + // extract bundles + std::stringstream ss(bundle_string); + std::string token; + while (std::getline(ss, token, ' ')) { + if (!token.empty()) { + bundle_files.push_back(token); + } + } + n_bundles = bundle_files.size(); + + // Camera input topics. Use remapping to map to your camera topics. + cam_image_topic = "camera_image"; + cam_info_topic = "camera_info"; } - // Get params from command line - marker_size = atof(argv[1]); - max_new_marker_error = atof(argv[2]); - max_track_error = atof(argv[3]); - cam_image_topic = argv[4]; - cam_info_topic = argv[5]; - output_frame = argv[6]; - med_filt_size = atoi(argv[7]); - int n_args_before_list = 8; - n_bundles = argc - n_args_before_list; - - marker_detector.SetMarkerSize(marker_size); + // Set dynamically configurable parameters so they don't get replaced by default values + pn.setParam("max_frequency", max_frequency); + pn.setParam("marker_size", marker_size); + pn.setParam("max_new_marker_error", max_new_marker_error); + pn.setParam("max_track_error", max_track_error); + + marker_detector.SetMarkerSize(marker_size, marker_resolution, marker_margin); multi_marker_bundles = new MultiMarkerBundle*[n_bundles]; bundlePoses = new Pose[n_bundles]; master_id = new int[n_bundles]; @@ -802,19 +864,19 @@ int main(int argc, char *argv[]) med_filts[i] = new ata::MedianFilter(med_filt_size); // 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]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML); + multi_marker_bundles[i]->Load(bundle_files[i].c_str(), 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; + else { + cout<<"Cannot load file "<< bundle_files[i] << endl; return 0; } } @@ -827,15 +889,49 @@ int main(int argc, char *argv[]) rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0); rvizMarkerPub2_ = n.advertise < visualization_msgs::Marker > ("ARmarker_points", 0); + // Prepare dynamic reconfiguration + dynamic_reconfigure::Server < ar_track_alvar::ParamsConfig > server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&configCallback, _1, _2); + server.setCallback(f); + //Give tf a chance to catch up before the camera callback starts asking for transforms ros::Duration(1.0).sleep(); ros::spinOnce(); - //Subscribe to topics and set up callbacks - ROS_INFO ("Subscribing to image topic"); - cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback); + // Run at the configured rate, discarding pointcloud msgs if necessary + ros::Rate rate(max_frequency); + + /// Subscriber for enable-topic so that a user can turn off the detection if it is not used without + /// having to use the reconfigure where he has to know all parameters + ros::Subscriber enable_sub_ = pn.subscribe("enable_detection", 1, &enableCallback); + + enable_switched = true; + while (ros::ok()) { + ros::spinOnce(); + rate.sleep(); - ros::spin(); + if (std::abs((rate.expectedCycleTime() - ros::Duration(1.0 / max_frequency)).toSec()) > 0.001) + { + // Change rate dynamically; if must be above 0, as 0 will provoke a segfault on next spinOnce + ROS_DEBUG("Changing frequency from %.2f to %.2f", 1.0 / rate.expectedCycleTime().toSec(), max_frequency); + rate = ros::Rate(max_frequency); + } + + if (enable_switched) + { + // Enable/disable switch: subscribe/unsubscribe to make use of pointcloud processing nodelet + // lazy publishing policy; in CPU-scarce computer as TurtleBot's laptop this is a huge saving + if (enabled) { + ROS_INFO ("Subscribing to cloud topic"); + cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback); + } + else + cloud_sub_.shutdown(); + enable_switched = false; + } + } return 0; } From 1323043ebad8bec44fa6cdd99ba4b8384a292286 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Wed, 11 Nov 2020 09:46:21 +0000 Subject: [PATCH 07/22] add pre-commit config --- .cmake-format.yaml | 33 +++++++++++++++++++++++++++++++++ .pre-commit-config.yaml | 10 ++++++++++ 2 files changed, 43 insertions(+) create mode 100644 .cmake-format.yaml create mode 100644 .pre-commit-config.yaml diff --git a/.cmake-format.yaml b/.cmake-format.yaml new file mode 100644 index 0000000..e70e353 --- /dev/null +++ b/.cmake-format.yaml @@ -0,0 +1,33 @@ +additional_commands: + foo: + flags: + - BAR + - BAZ + kwargs: + DEPENDS: '*' + HEADERS: '*' + SOURCES: '*' +algorithm_order: +- 0 +- 1 +- 2 +- 3 +always_wrap: [] +bullet_char: '*' +command_case: lower +dangle_parens: false +emit_byteorder_mark: false +enable_markup: false +enum_char: . +fence_pattern: ^\s*([`~]{3}[`~]*)(.*)$ +first_comment_is_literal: false +keyword_case: upper +line_ending: unix +line_width: 80 +literal_comment_pattern: null +max_subargs_per_line: 3 +per_command: {} +ruler_pattern: ^\s*[^\w\s]{3}.*[^\w\s]{3}$ +separate_ctrl_name_with_space: false +separate_fn_name_with_space: false +tab_size: 2 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..12e6329 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,10 @@ +- repo: local + hooks: + - id: clang-format + name: Run clang-format + description: Clang C/C++ code formatter. + entry: clang-format -i + args: [-style=file] + language: system + types: [file, c++] + # note: formatting style in .clang-format From d21bdbb66d07c3459cb1fc539eb9165c44dd7e0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Wed, 11 Nov 2020 09:50:04 +0000 Subject: [PATCH 08/22] reformat files --- ar_track_alvar/include/ar_track_alvar/Alvar.h | 155 +- .../include/ar_track_alvar/AlvarException.h | 10 +- .../include/ar_track_alvar/Bitset.h | 187 +- .../include/ar_track_alvar/Camera.h | 490 ++-- .../include/ar_track_alvar/Capture.h | 290 +- .../include/ar_track_alvar/CaptureDevice.h | 72 +- .../include/ar_track_alvar/CaptureFactory.h | 172 +- .../ar_track_alvar/CaptureFactory_private.h | 44 +- .../include/ar_track_alvar/CapturePlugin.h | 72 +- .../ar_track_alvar/ConnectedComponents.h | 150 +- .../include/ar_track_alvar/Container3d.h | 408 +-- .../include/ar_track_alvar/CvTestbed.h | 234 +- .../ar_track_alvar/DirectoryIterator.h | 69 +- .../DirectoryIterator_private.h | 26 +- ar_track_alvar/include/ar_track_alvar/Draw.h | 238 +- ar_track_alvar/include/ar_track_alvar/EC.h | 1545 ++++++----- .../ar_track_alvar/FernImageDetector.h | 100 +- .../ar_track_alvar/FernPoseEstimator.h | 37 +- .../include/ar_track_alvar/FileFormat.h | 69 +- .../include/ar_track_alvar/FileFormatUtils.h | 86 +- .../include/ar_track_alvar/Filter.h | 279 +- .../include/ar_track_alvar/GlutViewer.h | 82 +- .../include/ar_track_alvar/IntegralImage.h | 213 +- .../include/ar_track_alvar/Kalman.h | 518 ++-- ar_track_alvar/include/ar_track_alvar/Line.h | 66 +- ar_track_alvar/include/ar_track_alvar/Lock.h | 41 +- .../include/ar_track_alvar/Marker.h | 639 +++-- .../include/ar_track_alvar/MarkerDetector.h | 235 +- .../include/ar_track_alvar/MultiMarker.h | 312 ++- .../ar_track_alvar/MultiMarkerBundle.h | 118 +- .../ar_track_alvar/MultiMarkerFiltered.h | 84 +- .../ar_track_alvar/MultiMarkerInitializer.h | 170 +- ar_track_alvar/include/ar_track_alvar/Mutex.h | 46 +- .../include/ar_track_alvar/Mutex_private.h | 16 +- .../include/ar_track_alvar/Optimization.h | 190 +- .../include/ar_track_alvar/Platform.h | 14 +- .../include/ar_track_alvar/Plugin.h | 87 +- .../include/ar_track_alvar/Plugin_private.h | 18 +- ar_track_alvar/include/ar_track_alvar/Pose.h | 149 +- .../include/ar_track_alvar/Ransac.h | 712 ++--- .../include/ar_track_alvar/Rotation.h | 364 +-- ar_track_alvar/include/ar_track_alvar/SfM.h | 179 +- .../include/ar_track_alvar/Shared.h | 63 +- .../include/ar_track_alvar/Threads.h | 38 +- .../include/ar_track_alvar/Threads_private.h | 14 +- ar_track_alvar/include/ar_track_alvar/Timer.h | 40 +- .../include/ar_track_alvar/Timer_private.h | 16 +- .../include/ar_track_alvar/Tracker.h | 27 +- .../include/ar_track_alvar/TrackerFeatures.h | 163 +- .../ar_track_alvar/TrackerOrientation.h | 147 +- .../include/ar_track_alvar/TrackerPsa.h | 76 +- .../include/ar_track_alvar/TrackerStat.h | 74 +- .../include/ar_track_alvar/TrifocalTensor.h | 212 +- .../include/ar_track_alvar/Uncopyable.h | 46 +- .../include/ar_track_alvar/UnscentedKalman.h | 466 ++-- ar_track_alvar/include/ar_track_alvar/Util.h | 609 +++-- .../ar_track_alvar/filter/kinect_filtering.h | 45 +- .../ar_track_alvar/filter/medianFilter.h | 24 +- ar_track_alvar/nodes/FindMarkerBundles.cpp | 994 ++++--- .../nodes/FindMarkerBundlesNoKinect.cpp | 408 +-- ar_track_alvar/nodes/IndividualMarkers.cpp | 573 ++-- .../nodes/IndividualMarkersNoKinect.cpp | 410 +-- ar_track_alvar/nodes/TrainMarkerBundle.cpp | 733 ++--- ar_track_alvar/src/Alvar.cpp | 30 +- ar_track_alvar/src/Bitset.cpp | 604 +++-- ar_track_alvar/src/Camera.cpp | 1682 ++++++------ ar_track_alvar/src/CaptureDevice.cpp | 26 +- ar_track_alvar/src/CaptureFactory.cpp | 405 +-- ar_track_alvar/src/CaptureFactory_unix.cpp | 103 +- ar_track_alvar/src/CaptureFactory_win.cpp | 122 +- ar_track_alvar/src/ConnectedComponents.cpp | 706 ++--- ar_track_alvar/src/CvTestbed.cpp | 427 +-- ar_track_alvar/src/DirectoryIterator.cpp | 20 +- ar_track_alvar/src/DirectoryIterator_unix.cpp | 125 +- ar_track_alvar/src/DirectoryIterator_win.cpp | 114 +- ar_track_alvar/src/Draw.cpp | 453 ++-- ar_track_alvar/src/EC.cpp | 459 ++-- ar_track_alvar/src/FernImageDetector.cpp | 854 +++--- ar_track_alvar/src/FernPoseEstimator.cpp | 58 +- ar_track_alvar/src/FileFormatUtils.cpp | 186 +- ar_track_alvar/src/Filter.cpp | 153 +- ar_track_alvar/src/GlutViewer.cpp | 721 ++--- ar_track_alvar/src/IntegralImage.cpp | 345 +-- ar_track_alvar/src/Kalman.cpp | 931 ++++--- ar_track_alvar/src/Line.cpp | 154 +- ar_track_alvar/src/Marker.cpp | 2369 ++++++++++------- ar_track_alvar/src/MarkerDetector.cpp | 378 +-- ar_track_alvar/src/MultiMarker.cpp | 630 +++-- ar_track_alvar/src/MultiMarkerBundle.cpp | 457 ++-- ar_track_alvar/src/MultiMarkerFiltered.cpp | 126 +- ar_track_alvar/src/MultiMarkerInitializer.cpp | 261 +- ar_track_alvar/src/Mutex.cpp | 15 +- ar_track_alvar/src/Mutex_unix.cpp | 28 +- ar_track_alvar/src/Mutex_win.cpp | 28 +- ar_track_alvar/src/Optimization.cpp | 524 ++-- ar_track_alvar/src/Platform.cpp | 70 +- ar_track_alvar/src/Plugin.cpp | 43 +- ar_track_alvar/src/Plugin_unix.cpp | 55 +- ar_track_alvar/src/Plugin_win.cpp | 56 +- ar_track_alvar/src/Pose.cpp | 240 +- ar_track_alvar/src/Ransac.cpp | 280 +- ar_track_alvar/src/Rotation.cpp | 521 ++-- ar_track_alvar/src/SampleCamCalib.cpp | 372 +-- ar_track_alvar/src/SampleCvTestbed.cpp | 267 +- ar_track_alvar/src/SampleFilter.cpp | 459 ++-- ar_track_alvar/src/SampleIntegralImage.cpp | 430 +-- ar_track_alvar/src/SampleLabeling.cpp | 316 ++- ar_track_alvar/src/SampleMarkerCreator.cpp | 672 +++-- ar_track_alvar/src/SampleMarkerDetector.cpp | 408 +-- ar_track_alvar/src/SampleMarkerHide.cpp | 438 +-- .../src/SampleMarkerlessCreator.cpp | 100 +- .../src/SampleMarkerlessDetector.cpp | 384 +-- ar_track_alvar/src/SampleMultiMarker.cpp | 419 +-- .../src/SampleMultiMarkerBundle.cpp | 567 ++-- ar_track_alvar/src/SampleOptimization.cpp | 263 +- ar_track_alvar/src/SamplePointcloud.cpp | 541 ++-- ar_track_alvar/src/SampleTrack.cpp | 469 ++-- ar_track_alvar/src/SfM.cpp | 1064 ++++---- ar_track_alvar/src/Threads.cpp | 15 +- ar_track_alvar/src/Threads_unix.cpp | 44 +- ar_track_alvar/src/Threads_win.cpp | 73 +- ar_track_alvar/src/Timer.cpp | 15 +- ar_track_alvar/src/Timer_unix.cpp | 30 +- ar_track_alvar/src/Timer_win.cpp | 78 +- ar_track_alvar/src/TrackerFeatures.cpp | 503 ++-- ar_track_alvar/src/TrackerOrientation.cpp | 422 +-- ar_track_alvar/src/TrackerPsa.cpp | 438 +-- ar_track_alvar/src/TrackerStat.cpp | 160 +- ar_track_alvar/src/TrifocalTensor.cpp | 223 +- ar_track_alvar/src/UnscentedKalman.cpp | 291 +- ar_track_alvar/src/Util.cpp | 1069 ++++---- ar_track_alvar/src/kinect_filtering.cpp | 400 ++- ar_track_alvar/src/medianFilter.cpp | 103 +- .../capture_plugin_cmu/CapturePluginCmu.cpp | 441 +-- .../capture_plugin_cmu/CapturePluginCmu.h | 95 +- .../CapturePluginDSCapture.cpp | 255 +- .../CapturePluginDSCapture.h | 142 +- .../capture_plugin_file/CapturePluginFile.cpp | 116 +- .../capture_plugin_file/CapturePluginFile.h | 95 +- .../CapturePluginHighgui.cpp | 175 +- .../CapturePluginHighgui.h | 98 +- .../CapturePluginPtgrey.cpp | 319 ++- .../CapturePluginPtgrey.h | 108 +- ar_track_alvar/test/test_kinect_filtering.cpp | 50 +- ar_track_alvar/test/test_points.cpp | 34 +- 145 files changed, 23003 insertions(+), 18881 deletions(-) mode change 100755 => 100644 ar_track_alvar/include/ar_track_alvar/DirectoryIterator.h mode change 100755 => 100644 ar_track_alvar/include/ar_track_alvar/DirectoryIterator_private.h mode change 100755 => 100644 ar_track_alvar/include/ar_track_alvar/Shared.h mode change 100755 => 100644 ar_track_alvar/src/DirectoryIterator.cpp mode change 100755 => 100644 ar_track_alvar/src/DirectoryIterator_unix.cpp mode change 100755 => 100644 ar_track_alvar/src/DirectoryIterator_win.cpp diff --git a/ar_track_alvar/include/ar_track_alvar/Alvar.h b/ar_track_alvar/include/ar_track_alvar/Alvar.h index 3418557..0e5da2c 100644 --- a/ar_track_alvar/include/ar_track_alvar/Alvar.h +++ b/ar_track_alvar/include/ar_track_alvar/Alvar.h @@ -29,44 +29,52 @@ * * \section Introduction * - * ALVAR is a software library for creating virtual and augmented reality (AR) applications. ALVAR has - * been developed by the VTT Technical Research Centre of Finland. ALVAR is released under the terms of - * the GNU Lesser General Public License, version 2.1, or (at your option) any later version. - * - * ALVAR is designed to be as flexible as possible. It offers high-level tools and methods for creating - * augmented reality applications with just a few lines of code. The library also includes interfaces - * for all of the low-level tools and methods, which makes it possible for the user to develop their - * own solutions using alternative approaches or completely new algorithms. - * - * ALVAR is currently provided on Windows and Linux operating systems and only depends on one third - * party library (OpenCV). ALVAR is independent of any graphical libraries and can be easily integrated - * into existing applications. The sample applications use GLUT and the demo applications use OpenSceneGraph. + * ALVAR is a software library for creating virtual and augmented reality (AR) + * applications. ALVAR has been developed by the VTT Technical Research Centre + * of Finland. ALVAR is released under the terms of the GNU Lesser General + * Public License, version 2.1, or (at your option) any later version. + * + * ALVAR is designed to be as flexible as possible. It offers high-level tools + * and methods for creating augmented reality applications with just a few lines + * of code. The library also includes interfaces for all of the low-level tools + * and methods, which makes it possible for the user to develop their own + * solutions using alternative approaches or completely new algorithms. + * + * ALVAR is currently provided on Windows and Linux operating systems and only + * depends on one third party library (OpenCV). ALVAR is independent of any + * graphical libraries and can be easily integrated into existing applications. + * The sample applications use GLUT and the demo applications use + * OpenSceneGraph. * * \section Features * - * - Detecting and tracking 2D markers (\e MarkerDetector). Currently two types of square matrix markers - * are supported (\e MarkerData and \e MarkerArtoolkit). Future marker types can easily be added. ALVAR - * keeps the \e Marker \e Pose estimation as accurate as possible. Furthermore, ALVAR uses some tracking - * heuristics to identify markers that are "too far" and to recover from occlusions in the multimarker - * case for example. - * - Using a setup of multiple markers for pose detection (\e MultiMarker). The marker setup coordinates - * can be set manually or they can be automatically deduced using various methods (\e MultiMarkerFiltered - * and \e MultiMarkerBundle). - * - Tools for calibrating \e Camera. Distorting and undistorting points, projecting points and finding - * exterior orientation using point-sets. + * - Detecting and tracking 2D markers (\e MarkerDetector). Currently two types + * of square matrix markers are supported (\e MarkerData and \e + * MarkerArtoolkit). Future marker types can easily be added. ALVAR keeps the \e + * Marker \e Pose estimation as accurate as possible. Furthermore, ALVAR uses + * some tracking heuristics to identify markers that are "too far" and to + * recover from occlusions in the multimarker case for example. + * - Using a setup of multiple markers for pose detection (\e MultiMarker). The + * marker setup coordinates can be set manually or they can be automatically + * deduced using various methods (\e MultiMarkerFiltered and \e + * MultiMarkerBundle). + * - Tools for calibrating \e Camera. Distorting and undistorting points, + * projecting points and finding exterior orientation using point-sets. * - Hiding markers from the view (\e BuildHideTexture and \e DrawTexture). - * - Several basic filters: \e FilterAverage, \e FilterMedian, \e FilterRunningAverage, - * \e FilterDoubleExponentialSmoothing. - * - \e Kalman filters for sensor fusion: \e Kalman Filter, \e Extended Kalman Filter and Unscented Kalman - * Filter (\e KalmanSensor, \e KalmanSensorEkf, \e KalmanEkf, \e UnscentedKalman). - * - Several methods for tracking using optical flow: \e TrackerPsa , \e TrackerPsaRot , \e TrackerFeatures - * and \e TrackerStat. + * - Several basic filters: \e FilterAverage, \e FilterMedian, \e + * FilterRunningAverage, \e FilterDoubleExponentialSmoothing. + * - \e Kalman filters for sensor fusion: \e Kalman Filter, \e Extended Kalman + * Filter and Unscented Kalman Filter (\e KalmanSensor, \e KalmanSensorEkf, \e + * KalmanEkf, \e UnscentedKalman). + * - Several methods for tracking using optical flow: \e TrackerPsa , \e + * TrackerPsaRot , \e TrackerFeatures and \e TrackerStat. * - etc... * * \section Platforms * * ALVAR is officially supported and tested on the following platforms. - * - Windows XP 32-bit, Microsoft Visual Studio 2005 (8.0), 2008 (9.0) and 2010 (10.0) + * - Windows XP 32-bit, Microsoft Visual Studio 2005 (8.0), 2008 (9.0) and 2010 + * (10.0) * - Linux 32-bit, GCC 4.3 and 4.4 * - Linux 64-bit, GCC 4.3 and 4.4 * @@ -96,61 +104,68 @@ * - OpenSceneGraph (http://www.openscenegraph.org) * * \example SampleCamCalib.cpp - * This is an example of how to use \e ProjPoints and \e Camera classes to perform camera calibration - * using a chessboard pattern. + * This is an example of how to use \e ProjPoints and \e Camera classes to + * perform camera calibration using a chessboard pattern. * * \example SampleCvTestbed.cpp - * This is an example of how to use the \e CvTestbed and \e Capture classes in order to make quick OpenCV - * prototype applications. + * This is an example of how to use the \e CvTestbed and \e Capture classes in + * order to make quick OpenCV prototype applications. * * \example SampleFilter.cpp - * This is an example of how to use various filters: \e FilterAverage, \e FilterMedian, - * \e FilterRunningAverage, \e FilterDoubleExponentialSmoothing and \e Kalman. + * This is an example of how to use various filters: \e FilterAverage, \e + * FilterMedian, \e FilterRunningAverage, \e FilterDoubleExponentialSmoothing + * and \e Kalman. * * \example SampleIntegralImage.cpp - * This is an example of how to use the \e IntegralImage and \e IntegralGradient classes for image - * gradient analysis. + * This is an example of how to use the \e IntegralImage and \e IntegralGradient + * classes for image gradient analysis. * * \example SampleLabeling.cpp - * This is an example of how to label images using \e LabelImage and \e MarchEdge. + * This is an example of how to label images using \e LabelImage and \e + * MarchEdge. * * \example SampleMarkerCreator.cpp - * This is an example that demonstrates the generation of \e MarkerData markers and saving the image - * using \e SaveMarkerImage. + * This is an example that demonstrates the generation of \e MarkerData markers + * and saving the image using \e SaveMarkerImage. * * \example SampleMarkerDetector.cpp - * This is an example that shows how to detect \e MarkerData markers and visualize them using\e GlutViewer. + * This is an example that shows how to detect \e MarkerData markers and + * visualize them using\e GlutViewer. * * \example SampleMarkerHide.cpp - * This is an example that shows how to detect \e MarkerData markers, visualize them using \e GlutViewer - * and hide them with \e BuildHideTexture and \e DrawTexture. + * This is an example that shows how to detect \e MarkerData markers, visualize + * them using \e GlutViewer and hide them with \e BuildHideTexture and \e + * DrawTexture. * * \example SampleMarkerlessCreator.cpp - * This is an example that demonstrates the use of FernImageDetector to train a Fern classifier. + * This is an example that demonstrates the use of FernImageDetector to train a + * Fern classifier. * * \example SampleMarkerlessDetector.cpp - * This is an example that demonstrates the use of FernImageDetector to detect an image as a marker. + * This is an example that demonstrates the use of FernImageDetector to detect + * an image as a marker. * * \example SampleMultiMarker.cpp - * This is an example that demonstrates the use of a preconfigured \e MultiMarker setup. + * This is an example that demonstrates the use of a preconfigured \e + * MultiMarker setup. * * \example SampleMultiMarkerBundle.cpp - * This is an example that automatically recognising \e MultiMarker setups using \e MultiMarkerFiltered and - * optimizes it with \e MultiMarkerBundle. + * This is an example that automatically recognising \e MultiMarker setups using + * \e MultiMarkerFiltered and optimizes it with \e MultiMarkerBundle. * * \example SampleOptimization.cpp - * This is an example of how to use the \e Optimization class by fitting curves of increasing degree to - * random data. + * This is an example of how to use the \e Optimization class by fitting curves + * of increasing degree to random data. * * \example SamplePointcloud.cpp - * This is an example showing how to use \e SimpleSfM for tracking the environment using features in - * addition to \e MultiMarker. + * This is an example showing how to use \e SimpleSfM for tracking the + * environment using features in addition to \e MultiMarker. * * \example SampleTrack.cpp - * This is an example that shows how to perform tracking of the optical flow using \e TrackerPsa, - * \e TrackerPsaRot, \e TrackerFeatures or \e TrackerStat. + * This is an example that shows how to perform tracking of the optical flow + * using \e TrackerPsa, \e TrackerPsaRot, \e TrackerFeatures or \e TrackerStat. */ - + /** * \file Alvar.h * @@ -159,20 +174,20 @@ */ #if defined(WIN32) && !defined(ALVAR_STATIC) - #ifdef ALVAR_BUILD - #define ALVAR_EXPORT __declspec(dllexport) - #else - #define ALVAR_EXPORT __declspec(dllimport) - #endif +#ifdef ALVAR_BUILD +#define ALVAR_EXPORT __declspec(dllexport) #else - #define ALVAR_EXPORT +#define ALVAR_EXPORT __declspec(dllimport) +#endif +#else +#define ALVAR_EXPORT #endif /** * \brief Main ALVAR namespace. */ -namespace alvar { - +namespace alvar +{ /** * \brief Major version number. */ @@ -193,35 +208,35 @@ static const int ALVAR_VERSION_PATCH = 0; * * The tag contains alpha, beta and release candidate versions. */ -static const char *ALVAR_VERSION_TAG = ""; +static const char* ALVAR_VERSION_TAG = ""; /** * \brief Revision version string. * * The revision contains an identifier from the source control system. */ -static const char *ALVAR_VERSION_REVISION = ""; +static const char* ALVAR_VERSION_REVISION = ""; /** * \brief Entire version string. */ -static const char *ALVAR_VERSION = "2.0.0"; +static const char* ALVAR_VERSION = "2.0.0"; /** * \brief Entire version string without dots. */ -static const char *ALVAR_VERSION_NODOTS = "200"; +static const char* ALVAR_VERSION_NODOTS = "200"; /** * \brief Date the library was built. */ -static const char *ALVAR_DATE = "2012-06-20"; +static const char* ALVAR_DATE = "2012-06-20"; /** * \brief System the library was built on. */ -static const char *ALVAR_SYSTEM = "Linux 3.2.0-24-generic x86_64"; +static const char* ALVAR_SYSTEM = "Linux 3.2.0-24-generic x86_64"; -} +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/AlvarException.h b/ar_track_alvar/include/ar_track_alvar/AlvarException.h index 21b3c1a..99c971d 100644 --- a/ar_track_alvar/include/ar_track_alvar/AlvarException.h +++ b/ar_track_alvar/include/ar_track_alvar/AlvarException.h @@ -32,17 +32,19 @@ #include -namespace alvar { - +namespace alvar +{ /** * \brief ALVAR exception class. */ class AlvarException : public std::runtime_error { public: - AlvarException(const char *s) : std::runtime_error(s) { } + AlvarException(const char* s) : std::runtime_error(s) + { + } }; -} +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Bitset.h b/ar_track_alvar/include/ar_track_alvar/Bitset.h index 5cc9df8..35ba5d8 100644 --- a/ar_track_alvar/include/ar_track_alvar/Bitset.h +++ b/ar_track_alvar/include/ar_track_alvar/Bitset.h @@ -37,14 +37,16 @@ #include #include -namespace alvar { - +namespace alvar +{ /** * \brief \e Bitset is a basic class for handling bit sequences * - * The bits are stored internally using deque (See http://osdir.com/ml/gis.geos.devel/2006-04/msg00142.html). - * The bitset is assumed to have most significant bits left i.e. the push_back() methods add to the least - * significant end of the bit sequence. The usage is clarified by the following example. + * The bits are stored internally using deque (See + * http://osdir.com/ml/gis.geos.devel/2006-04/msg00142.html). The bitset is + * assumed to have most significant bits left i.e. the push_back() methods add + * to the least significant end of the bit sequence. The usage is clarified by + * the following example. * * \section Usage * \code @@ -59,101 +61,110 @@ namespace alvar { * b.Output(std::cout); // b contains now: 00000000 00000000 10001000 10001000 * \endcode */ -class ALVAR_EXPORT Bitset { +class ALVAR_EXPORT Bitset +{ protected: - std::deque bits; - + std::deque bits; + public: - /** \brief The length of the \e Bitset */ - int Length(); - /** \brief Output the bits to selected ostream - * \param os The output stream to be used for outputting e.g. std::cout - */ - std::ostream &Output(std::ostream &os) const; - /** \brief Clear the bits */ - void clear(); - /** \brief Push back one bit - * \param bit Boolean (true/false) to be pushed to the end of bit sequence. - */ - void push_back(const bool bit); - /** \brief Push back \e bit_count bits from 'byte' \e b - * \param b Unsigned character (8-bits) to be pushed to the end of bit sequence. - * \param bit_count Number of bits to be pushed (default/max is 8 bits) - */ - void push_back(const unsigned char b, const int bit_count=8); - /** \brief Push back \e bit_count bits from 'short' \e s - * \param s Unsigned short (16-bits) to be pushed to the end of bit sequence. - * \param bit_count Number of bits to be pushed (default/max is 16 bits) - */ - void push_back(const unsigned short s, const int bit_count=16); - /** \brief Push back \e bit_count bits from 'long' \e l - * \param l Unsigned long (32-bits) to be pushed to the end of bit sequence. - * \param bit_count Number of bits to be pushed (default/max is 32 bits) - */ - void push_back(const unsigned long l, const int bit_count=32); - /** \brief Push back meaningful bits from 'long' \e l - * \param l The meaningful bits of the given unsigned long (32-bits) are pushed to the end of bit sequence. - */ - void push_back_meaningful(const unsigned long l); - /** \brief Fill the \e Bitset with non-meaningful zeros - * \param bit_count Non-meaningful zeros are added until this given \e bit_count is reached. - */ - void fill_zeros_left(const size_t bit_count); - /** \brief Push back a string of characters - * \param s String of characters to be pushed to the end of bit sequence. - */ - void push_back(std::string s); - /** \brief Pop the front bit */ - bool pop_front(); - /** \brief Pop the back bit */ - bool pop_back(); - /** \brief Flip the selected bit - * \param pos The bit in this given position is flipped. - */ - void flip(size_t pos); - /** \brief The \e Bitset as a hex string */ - std::string hex(); - /** \brief The \e Bitset as 'unsigned long' */ - unsigned long ulong(); - /** \brief The \e Bitset as 'unsigned char' */ - unsigned char uchar(); - /** \brief The \e Bitset as 'deque' */ - inline std::deque& GetBits() - { - return bits; - } + /** \brief The length of the \e Bitset */ + int Length(); + /** \brief Output the bits to selected ostream + * \param os The output stream to be used for outputting e.g. std::cout + */ + std::ostream& Output(std::ostream& os) const; + /** \brief Clear the bits */ + void clear(); + /** \brief Push back one bit + * \param bit Boolean (true/false) to be pushed to the end of bit sequence. + */ + void push_back(const bool bit); + /** \brief Push back \e bit_count bits from 'byte' \e b + * \param b Unsigned character (8-bits) to be pushed to the end of bit + * sequence. \param bit_count Number of bits to be pushed (default/max is 8 + * bits) + */ + void push_back(const unsigned char b, const int bit_count = 8); + /** \brief Push back \e bit_count bits from 'short' \e s + * \param s Unsigned short (16-bits) to be pushed to the end of bit sequence. + * \param bit_count Number of bits to be pushed (default/max is 16 bits) + */ + void push_back(const unsigned short s, const int bit_count = 16); + /** \brief Push back \e bit_count bits from 'long' \e l + * \param l Unsigned long (32-bits) to be pushed to the end of bit sequence. + * \param bit_count Number of bits to be pushed (default/max is 32 bits) + */ + void push_back(const unsigned long l, const int bit_count = 32); + /** \brief Push back meaningful bits from 'long' \e l + * \param l The meaningful bits of the given unsigned long (32-bits) are + * pushed to the end of bit sequence. + */ + void push_back_meaningful(const unsigned long l); + /** \brief Fill the \e Bitset with non-meaningful zeros + * \param bit_count Non-meaningful zeros are added until this given \e + * bit_count is reached. + */ + void fill_zeros_left(const size_t bit_count); + /** \brief Push back a string of characters + * \param s String of characters to be pushed to the end of bit sequence. + */ + void push_back(std::string s); + /** \brief Pop the front bit */ + bool pop_front(); + /** \brief Pop the back bit */ + bool pop_back(); + /** \brief Flip the selected bit + * \param pos The bit in this given position is flipped. + */ + void flip(size_t pos); + /** \brief The \e Bitset as a hex string */ + std::string hex(); + /** \brief The \e Bitset as 'unsigned long' */ + unsigned long ulong(); + /** \brief The \e Bitset as 'unsigned char' */ + unsigned char uchar(); + /** \brief The \e Bitset as 'deque' */ + inline std::deque& GetBits() + { + return bits; + } }; /** - * \brief An extended \e Bitset ( \e BitsetExt ) for handling e.g. Hamming encoding/decoding + * \brief An extended \e Bitset ( \e BitsetExt ) for handling e.g. Hamming + * encoding/decoding * - * This class is based on the basic \e Bitset. It provides additional features for Hamming coding - * (See http://en.wikipedia.org/wiki/Hamming_code). + * This class is based on the basic \e Bitset. It provides additional features + * for Hamming coding (See http://en.wikipedia.org/wiki/Hamming_code). * * The \e BitsetExt is used e.g by \e MarkerData */ -class ALVAR_EXPORT BitsetExt : public Bitset { +class ALVAR_EXPORT BitsetExt : public Bitset +{ protected: - bool verbose; - void hamming_enc_block(unsigned long block_len, std::deque::iterator &iter); - int hamming_dec_block(unsigned long block_len, std::deque::iterator &iter); + bool verbose; + void hamming_enc_block(unsigned long block_len, + std::deque::iterator& iter); + int hamming_dec_block(unsigned long block_len, + std::deque::iterator& iter); + public: - /** \brief Constructor */ - BitsetExt(); - /** \brief Constructor */ - BitsetExt(bool _verbose); - /** \brief Set the verbose/silent mode */ - void SetVerbose(bool _verbose); - /** \brief Count how many bits will be in the Bitset after hamming encoding */ - static int count_hamming_enc_len(int block_len, int dec_len); - /** \brief Count how many bits will be in the Bitset after hamming decoding */ - static int count_hamming_dec_len(int block_len, int enc_len); - /** \brief Hamming encoding 'in-place' using the defined block length */ - void hamming_enc(int block_len); - /** \brief Hamming decoding 'in-place' using the defined block length */ - int hamming_dec(int block_len); + /** \brief Constructor */ + BitsetExt(); + /** \brief Constructor */ + BitsetExt(bool _verbose); + /** \brief Set the verbose/silent mode */ + void SetVerbose(bool _verbose); + /** \brief Count how many bits will be in the Bitset after hamming encoding */ + static int count_hamming_enc_len(int block_len, int dec_len); + /** \brief Count how many bits will be in the Bitset after hamming decoding */ + static int count_hamming_dec_len(int block_len, int enc_len); + /** \brief Hamming encoding 'in-place' using the defined block length */ + void hamming_enc(int block_len); + /** \brief Hamming decoding 'in-place' using the defined block length */ + int hamming_dec(int block_len); }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Camera.h b/ar_track_alvar/include/ar_track_alvar/Camera.h index beca09d..6fc6090 100644 --- a/ar_track_alvar/include/ar_track_alvar/Camera.h +++ b/ar_track_alvar/include/ar_track_alvar/Camera.h @@ -46,244 +46,290 @@ #include #include -namespace alvar { - -/** \brief Simple structure for collecting 2D and 3D points e.g. for camera calibration */ -struct ALVAR_EXPORT ProjPoints { - int width; - int height; - - /** \brief 3D object points corresponding with the detected 2D image points. */ - std::vector object_points; - /** \brief Detected 2D object points - * If point_counts[0] == 10, then the - * first 10 points are detected in the first frame. If - * point_counts[1] == 6, then the next 6 of these points are - * detected in the next frame... etc. - */ - std::vector image_points; - /** \brief Vector indicating how many points are detected for each frame */ - std::vector point_counts; - - /** \brief Reset \e object_points , \e image_points and \e point_counts */ - void Reset(); - - /** \brief Add elements to \e object_points , \e image_points and \e point_counts using Chessboard pattern */ - bool AddPointsUsingChessboard(IplImage *image, double etalon_square_size, int etalon_rows, int etalon_columns, bool visualize); - - /** \brief Add elements to \e object_points , \e image_points and \e point_counts using detected markers */ - bool AddPointsUsingMarkers(std::vector &marker_corners, std::vector &marker_corners_img, IplImage* image); +namespace alvar +{ +/** \brief Simple structure for collecting 2D and 3D points e.g. for camera + * calibration */ +struct ALVAR_EXPORT ProjPoints +{ + int width; + int height; + + /** \brief 3D object points corresponding with the detected 2D image points. + */ + std::vector object_points; + /** \brief Detected 2D object points + * If point_counts[0] == 10, then the + * first 10 points are detected in the first frame. If + * point_counts[1] == 6, then the next 6 of these points are + * detected in the next frame... etc. + */ + std::vector image_points; + /** \brief Vector indicating how many points are detected for each frame */ + std::vector point_counts; + + /** \brief Reset \e object_points , \e image_points and \e point_counts */ + void Reset(); + + /** \brief Add elements to \e object_points , \e image_points and \e + * point_counts using Chessboard pattern */ + bool AddPointsUsingChessboard(IplImage* image, double etalon_square_size, + int etalon_rows, int etalon_columns, + bool visualize); + + /** \brief Add elements to \e object_points , \e image_points and \e + * point_counts using detected markers */ + bool AddPointsUsingMarkers(std::vector& marker_corners, + std::vector& marker_corners_img, + IplImage* image); }; - /** - * \brief Simple \e Camera class for calculating distortions, orientation or projections with pre-calibrated camera + * \brief Simple \e Camera class for calculating distortions, orientation or + * projections with pre-calibrated camera */ -class ALVAR_EXPORT Camera { - +class ALVAR_EXPORT Camera +{ public: - - CvMat calib_K; double calib_K_data[3][3]; - CvMat calib_D; double calib_D_data[4]; - int calib_x_res; - int calib_y_res; - int x_res; - int y_res; - bool getCamInfo_; + CvMat calib_K; + double calib_K_data[3][3]; + CvMat calib_D; + double calib_D_data[4]; + int calib_x_res; + int calib_y_res; + int x_res; + int y_res; + bool getCamInfo_; protected: - std::string cameraInfoTopic_; - sensor_msgs::CameraInfo cam_info_; - void camInfoCallback (const sensor_msgs::CameraInfoConstPtr &); - ros::Subscriber sub_; - ros::NodeHandle n_; + std::string cameraInfoTopic_; + sensor_msgs::CameraInfo cam_info_; + void camInfoCallback(const sensor_msgs::CameraInfoConstPtr&); + ros::Subscriber sub_; + ros::NodeHandle n_; private: - bool LoadCalibXML(const char *calibfile); - bool LoadCalibOpenCV(const char *calibfile); - bool SaveCalibXML(const char *calibfile); - bool SaveCalibOpenCV(const char *calibfile); + bool LoadCalibXML(const char* calibfile); + bool LoadCalibOpenCV(const char* calibfile); + bool SaveCalibXML(const char* calibfile); + bool SaveCalibOpenCV(const char* calibfile); public: - /** \brief One of the two methods to make this class serializable by \e Serialization class */ - std::string SerializeId() { return "camera"; }; - /** \brief One of the two methods to make this class serializable by \e Serialization class - * - * You can serialize the \e Camera class using filename or any std::iostream - * as follows: - * \code - * alvar::Camera cam; - * cam.SetCalib("calib.xml", 320, 240); - * Serialization sero("calib1.xml"); - * sero<>cam; - * cam.SetRes(320, 240); - * \endcode - * \code - * std::stringstream ss; - * Serialization sero(ss); - * sero<>cam; - * \endcode - */ - bool Serialize(Serialization *ser) { - if (!ser->Serialize(calib_x_res, "width")) return false; - if (!ser->Serialize(calib_y_res, "height")) return false; - if (!ser->Serialize(calib_K, "intrinsic_matrix")) return false; - if (!ser->Serialize(calib_D, "distortion")) return false; - return true; - } - - /** \brief Constructor */ - Camera(); - Camera(ros::NodeHandle & n, std::string cam_info_topic); - - /** Sets the intrinsic calibration */ - void SetCameraInfo(const sensor_msgs::CameraInfo& camInfo); - - /** \brief Get x-direction FOV in radians */ - double GetFovX() { - return (2.0f * atan2(double(x_res) / 2.0f, (double)calib_K_data[0][0])); - } - /** \brief Get y-direction FOV in radians */ - double GetFovY() { - return (2.0f * atan2(double(y_res) / 2.0f, (double)calib_K_data[1][1])); - } - - void SetSimpleCalib(int _x_res, int _y_res, double f_fac=1.); - - /** \brief Set the calibration file and the current resolution for which the calibration is adjusted to - * \param calibfile File to load. - * \param _x_res Width of images captured from the real camera. - * \param _y_res Height of images captured from the real camera. - * \param format FILE_FORMAT_OPENCV (default) or FILE_FORMAT_XML (see doc/Camera.xsd). - */ - bool SetCalib(const char *calibfile, int _x_res, int _y_res, - FILE_FORMAT format = FILE_FORMAT_DEFAULT); - - /** \brief Save the current calibration information to a file - * \param calibfile File to save. - * \param format FILE_FORMAT_OPENCV (default) or FILE_FORMAT_XML (see doc/Camera.xsd). - */ - bool SaveCalib(const char *calibfile, FILE_FORMAT format = FILE_FORMAT_DEFAULT); - - /** \brief Calibrate using the collected \e ProjPoints */ - void Calibrate(ProjPoints &pp); - - /** \brief If we have no calibration file we can still adjust the default calibration to current resolution */ - void SetRes(int _x_res, int _y_res); - - /** \brief Get OpenGL matrix - * Generates the OpenGL projection matrix based on OpenCV intrinsic camera matrix K. - * \code - * 2*K[0][0]/width 2*K[0][1]/width -(2*K[0][2]/width+1) 0 - * 0 2*K[1][1]/height 2*K[1][2]/height-1 0 - * 0 0 -(f+n)/(f-n) -2*f*n/(f-n) - * 0 0 -1 0 - * \endcode - * - * Note, that the sign of the element [2][0] is changed. It should be - * \code - * 2*K[0][2]/width+1 - * \endcode - * - * The sign change is due to the fact that with OpenCV and OpenGL projection - * matrices both y and z should be mirrored. With other matrix elements - * the sign changes eliminate each other, but with principal point - * in x-direction we need to make the change by hand. - */ - void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height, const float far_clip = 1000.0f, const float near_clip = 0.1f); - - /** \brief Invert operation for \e GetOpenglProjectionMatrix */ - void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height); - - /** \brief Unapplys the lens distortion for points on image plane. */ - void Undistort(std::vector& points); - - /** \brief Unapplys the lens distortion for one point on an image plane. */ - void Undistort(PointDouble &point); - - /** \brief Unapplys the lens distortion for one point on an image plane. */ - void Undistort(CvPoint2D32f& point); - - /** \brief Applys the lens distortion for one point on an image plane. */ - void Distort(CvPoint2D32f& point); - - /** \brief Applys the lens distortion for points on image plane. */ - void Distort(std::vector& points); - - /** \brief Applys the lens distortion for points on image plane. */ - void Distort(PointDouble &point); - - /** \brief Calculate exterior orientation */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, Pose *pose); - - /** \brief Calculate exterior orientation - */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, - CvMat *rodriques, CvMat *tra); - - /** \brief Calculate exterior orientation - */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, - CvMat *rodriques, CvMat *tra); - - /** \brief Calculate exterior orientation - */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, Pose *pose); - - /** \brief Update existing pose based on new observations. Use (CV_32FC3 and CV_32FC2) for matrices. */ - bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, Pose *pose); - - /** \brief Update existing pose (in rodriques&tra) based on new observations. Use (CV_32FC3 and CV_32FC2) for matrices. */ - bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, CvMat *rodriques, CvMat *tra); - - /** \brief Project one point */ - void ProjectPoint(const CvPoint3D64f pw, const Pose *pose, CvPoint2D64f &pi) const; - - /** \brief Project one point */ - void ProjectPoint(const CvPoint3D32f pw, const Pose *pose, CvPoint2D32f &pi) const; - - /** \brief Project points */ - void ProjectPoints(std::vector& pw, Pose *pose, std::vector& pi) const; - - /** \brief Project points - */ - void ProjectPoints(const CvMat* object_points, const CvMat* rotation_vector, - const CvMat* translation_vector, CvMat* image_points) const; - - /** \brief Project points - */ - void ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const; - - /** \brief Project points */ - void ProjectPoints(const CvMat* object_points, const Pose* pose, CvMat* image_points) const; + /** \brief One of the two methods to make this class serializable by \e + * Serialization class */ + std::string SerializeId() + { + return "camera"; + }; + /** \brief One of the two methods to make this class serializable by \e + * Serialization class + * + * You can serialize the \e Camera class using filename or any std::iostream + * as follows: + * \code + * alvar::Camera cam; + * cam.SetCalib("calib.xml", 320, 240); + * Serialization sero("calib1.xml"); + * sero<>cam; + * cam.SetRes(320, 240); + * \endcode + * \code + * std::stringstream ss; + * Serialization sero(ss); + * sero<>cam; + * \endcode + */ + bool Serialize(Serialization* ser) + { + if (!ser->Serialize(calib_x_res, "width")) + return false; + if (!ser->Serialize(calib_y_res, "height")) + return false; + if (!ser->Serialize(calib_K, "intrinsic_matrix")) + return false; + if (!ser->Serialize(calib_D, "distortion")) + return false; + return true; + } + + /** \brief Constructor */ + Camera(); + Camera(ros::NodeHandle& n, std::string cam_info_topic); + + /** Sets the intrinsic calibration */ + void SetCameraInfo(const sensor_msgs::CameraInfo& camInfo); + + /** \brief Get x-direction FOV in radians */ + double GetFovX() + { + return (2.0f * atan2(double(x_res) / 2.0f, (double)calib_K_data[0][0])); + } + /** \brief Get y-direction FOV in radians */ + double GetFovY() + { + return (2.0f * atan2(double(y_res) / 2.0f, (double)calib_K_data[1][1])); + } + + void SetSimpleCalib(int _x_res, int _y_res, double f_fac = 1.); + + /** \brief Set the calibration file and the current resolution for which the + * calibration is adjusted to \param calibfile File to load. \param _x_res + * Width of images captured from the real camera. \param _y_res Height of + * images captured from the real camera. \param format FILE_FORMAT_OPENCV + * (default) or FILE_FORMAT_XML (see doc/Camera.xsd). + */ + bool SetCalib(const char* calibfile, int _x_res, int _y_res, + FILE_FORMAT format = FILE_FORMAT_DEFAULT); + + /** \brief Save the current calibration information to a file + * \param calibfile File to save. + * \param format FILE_FORMAT_OPENCV (default) or FILE_FORMAT_XML (see + * doc/Camera.xsd). + */ + bool SaveCalib(const char* calibfile, + FILE_FORMAT format = FILE_FORMAT_DEFAULT); + + /** \brief Calibrate using the collected \e ProjPoints */ + void Calibrate(ProjPoints& pp); + + /** \brief If we have no calibration file we can still adjust the default + * calibration to current resolution */ + void SetRes(int _x_res, int _y_res); + + /** \brief Get OpenGL matrix + * Generates the OpenGL projection matrix based on OpenCV intrinsic camera + * matrix K. \code 2*K[0][0]/width 2*K[0][1]/width -(2*K[0][2]/width+1) 0 + * 0 2*K[1][1]/height 2*K[1][2]/height-1 0 + * 0 0 -(f+n)/(f-n) -2*f*n/(f-n) + * 0 0 -1 0 + * \endcode + * + * Note, that the sign of the element [2][0] is changed. It should be + * \code + * 2*K[0][2]/width+1 + * \endcode + * + * The sign change is due to the fact that with OpenCV and OpenGL projection + * matrices both y and z should be mirrored. With other matrix elements + * the sign changes eliminate each other, but with principal point + * in x-direction we need to make the change by hand. + */ + void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, + const int height, + const float far_clip = 1000.0f, + const float near_clip = 0.1f); + + /** \brief Invert operation for \e GetOpenglProjectionMatrix */ + void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, + const int height); + + /** \brief Unapplys the lens distortion for points on image plane. */ + void Undistort(std::vector& points); + + /** \brief Unapplys the lens distortion for one point on an image plane. */ + void Undistort(PointDouble& point); + + /** \brief Unapplys the lens distortion for one point on an image plane. */ + void Undistort(CvPoint2D32f& point); + + /** \brief Applys the lens distortion for one point on an image plane. */ + void Distort(CvPoint2D32f& point); + + /** \brief Applys the lens distortion for points on image plane. */ + void Distort(std::vector& points); + + /** \brief Applys the lens distortion for points on image plane. */ + void Distort(PointDouble& point); + + /** \brief Calculate exterior orientation */ + void CalcExteriorOrientation(std::vector& pw, + std::vector& pi, Pose* pose); + + /** \brief Calculate exterior orientation + */ + void CalcExteriorOrientation(std::vector& pw, + std::vector& pi, CvMat* rodriques, + CvMat* tra); + + /** \brief Calculate exterior orientation + */ + void CalcExteriorOrientation(std::vector& pw, + std::vector& pi, CvMat* rodriques, + CvMat* tra); + + /** \brief Calculate exterior orientation + */ + void CalcExteriorOrientation(std::vector& pw, + std::vector& pi, Pose* pose); + + /** \brief Update existing pose based on new observations. Use (CV_32FC3 and + * CV_32FC2) for matrices. */ + bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, + Pose* pose); + + /** \brief Update existing pose (in rodriques&tra) based on new observations. + * Use (CV_32FC3 and CV_32FC2) for matrices. */ + bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, + CvMat* rodriques, CvMat* tra); + + /** \brief Project one point */ + void ProjectPoint(const CvPoint3D64f pw, const Pose* pose, + CvPoint2D64f& pi) const; + + /** \brief Project one point */ + void ProjectPoint(const CvPoint3D32f pw, const Pose* pose, + CvPoint2D32f& pi) const; + + /** \brief Project points */ + void ProjectPoints(std::vector& pw, Pose* pose, + std::vector& pi) const; + + /** \brief Project points + */ + void ProjectPoints(const CvMat* object_points, const CvMat* rotation_vector, + const CvMat* translation_vector, + CvMat* image_points) const; + + /** \brief Project points + */ + void ProjectPoints(const CvMat* object_points, double gl[16], + CvMat* image_points) const; + + /** \brief Project points */ + void ProjectPoints(const CvMat* object_points, const Pose* pose, + CvMat* image_points) const; }; /** - * \brief Simple \e Homography class for finding and projecting points between two planes. + * \brief Simple \e Homography class for finding and projecting points between + * two planes. */ -struct ALVAR_EXPORT Homography { - double H_data[3][3]; - CvMat H; - - /** \brief Constructor */ - Homography(); - - /** \brief Find Homography for two point-sets */ - void Find(const std::vector& pw, const std::vector& pi); - - /** \brief Project points using the Homography */ - void ProjectPoints(const std::vector& from, std::vector& to); +struct ALVAR_EXPORT Homography +{ + double H_data[3][3]; + CvMat H; + + /** \brief Constructor */ + Homography(); + + /** \brief Find Homography for two point-sets */ + void Find(const std::vector& pw, + const std::vector& pi); + + /** \brief Project points using the Homography */ + void ProjectPoints(const std::vector& from, + std::vector& to); }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Capture.h b/ar_track_alvar/include/ar_track_alvar/Capture.h index b6f3f90..7973327 100644 --- a/ar_track_alvar/include/ar_track_alvar/Capture.h +++ b/ar_track_alvar/include/ar_track_alvar/Capture.h @@ -34,155 +34,179 @@ #include "CaptureDevice.h" #include "Util.h" -namespace alvar { - +namespace alvar +{ /** * \brief Capture interface that plugins must implement. * - * All plugins must implement the Capture interface. This is the class that implements - * all of the camera capture funtionality. This class is created by the CapturePlugin - * implementation. + * All plugins must implement the Capture interface. This is the class that + * implements all of the camera capture funtionality. This class is created by + * the CapturePlugin implementation. */ class ALVAR_EXPORT Capture { public: - /** - * \brief Constructor. - * - * \param captureDevice Information of which camera to create. - */ - Capture(const CaptureDevice captureDevice) - : mCaptureDevice(captureDevice) - , mXResolution(0) - , mYResolution(0) - , mIsCapturing(false) + /** + * \brief Constructor. + * + * \param captureDevice Information of which camera to create. + */ + Capture(const CaptureDevice captureDevice) + : mCaptureDevice(captureDevice) + , mXResolution(0) + , mYResolution(0) + , mIsCapturing(false) + { + } + + /** + * \brief Destructor + */ + virtual ~Capture() + { + } + + /** + * \brief The camera information associated to this capture object. + */ + CaptureDevice captureDevice() + { + return mCaptureDevice; + } + + /** + * \brief The resolution along the x axis (horizontal). + */ + unsigned long xResolution() + { + return mXResolution; + } + + /** + * \brief The resolution along the y axis (vertical). + */ + unsigned long yResolution() + { + return mYResolution; + } + + /** + * \brief Test if the camera was properly initialized. + */ + bool isCapturing() + { + return mIsCapturing; + } + + /** + * \brief Set the resolution. + * + * \param xResolution The resolution along the x axis (horizontal). + * \param yResolution The resolution along the y axis (vertical). + */ + virtual void setResolution(const unsigned long xResolution, + const unsigned long yResolution) + { + } + + /** + * \brief Starts the camera capture. + * + * \return True if the camera was properly initialized, false otherwise. + */ + virtual bool start() = 0; + + /** + * \brief Stops the camera capture. + */ + virtual void stop() = 0; + + /** + * \brief Capture one image from the camera. + * + * Do not modify this image. + * + * \return The captured image. + */ + virtual IplImage* captureImage() = 0; + + /** + * \brief Save camera settings to a file. + * + * \param filename The filename to write to. + * \return True if the settings were sucessfully saved, false otherwise. + */ + virtual bool saveSettings(std::string filename) + { + if (!isCapturing()) { + return false; } - /** - * \brief Destructor - */ - virtual ~Capture() {} - - /** - * \brief The camera information associated to this capture object. - */ - CaptureDevice captureDevice() {return mCaptureDevice;} - - /** - * \brief The resolution along the x axis (horizontal). - */ - unsigned long xResolution() {return mXResolution;} - - /** - * \brief The resolution along the y axis (vertical). - */ - unsigned long yResolution() {return mYResolution;} - - /** - * \brief Test if the camera was properly initialized. - */ - bool isCapturing() {return mIsCapturing;} - - /** - * \brief Set the resolution. - * - * \param xResolution The resolution along the x axis (horizontal). - * \param yResolution The resolution along the y axis (vertical). - */ - virtual void setResolution(const unsigned long xResolution, const unsigned long yResolution) + Serialization serialization(filename); + try + { + serialization << (*this); + } + catch (...) + { + return false; + } + return true; + } + + /** + * \brief Load camera settings from a file. + * + * \param filename The filename to read from. + * \return True if the settings were sucessfully loaded, false otherwise. + */ + virtual bool loadSettings(std::string filename) + { + if (!isCapturing()) { + return false; } - /** - * \brief Starts the camera capture. - * - * \return True if the camera was properly initialized, false otherwise. - */ - virtual bool start() = 0; - - /** - * \brief Stops the camera capture. - */ - virtual void stop() = 0; - - /** - * \brief Capture one image from the camera. - * - * Do not modify this image. - * - * \return The captured image. - */ - virtual IplImage *captureImage() = 0; - - /** - * \brief Save camera settings to a file. - * - * \param filename The filename to write to. - * \return True if the settings were sucessfully saved, false otherwise. - */ - virtual bool saveSettings(std::string filename) { - if (!isCapturing()) { - return false; - } - - Serialization serialization(filename); - try { - serialization << (*this); - } - catch (...) { - return false; - } - return true; - } - - /** - * \brief Load camera settings from a file. - * - * \param filename The filename to read from. - * \return True if the settings were sucessfully loaded, false otherwise. - */ - virtual bool loadSettings(std::string filename) { - if (!isCapturing()) { - return false; - } - - Serialization serialization(filename); - try { - serialization >> (*this); - } - catch (...) { - return false; - } - return true; - } - - /** - * \brief Show the settings dialog of the camera. - * \return True if the settings dialog was shown, false otherwise. - */ - virtual bool showSettingsDialog() = 0; - - /** - * \brief The identification of the class for serialization. - */ - virtual std::string SerializeId() = 0; - - /** - * \brief Performs serialization of the class members and configuration. - * - * \param serialization The Serialization object. - * \return True if the serialization of the class was successful, false otherwise. - */ - virtual bool Serialize(Serialization *serialization) = 0; + Serialization serialization(filename); + try + { + serialization >> (*this); + } + catch (...) + { + return false; + } + return true; + } + + /** + * \brief Show the settings dialog of the camera. + * \return True if the settings dialog was shown, false otherwise. + */ + virtual bool showSettingsDialog() = 0; + + /** + * \brief The identification of the class for serialization. + */ + virtual std::string SerializeId() = 0; + + /** + * \brief Performs serialization of the class members and configuration. + * + * \param serialization The Serialization object. + * \return True if the serialization of the class was successful, false + * otherwise. + */ + virtual bool Serialize(Serialization* serialization) = 0; protected: - CaptureDevice mCaptureDevice; - unsigned long mXResolution; - unsigned long mYResolution; - bool mIsCapturing; + CaptureDevice mCaptureDevice; + unsigned long mXResolution; + unsigned long mYResolution; + bool mIsCapturing; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h b/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h index aae94ba..2beb21b 100644 --- a/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h +++ b/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h @@ -34,56 +34,58 @@ #include -namespace alvar { - +namespace alvar +{ /** * \brief CaptureDevice holder for camera information. * - * CaptureDevice contains the desired backend, the id and description of the camera. + * CaptureDevice contains the desired backend, the id and description of the + * camera. */ class ALVAR_EXPORT CaptureDevice { public: - /** - * \brief Constructor. - * - * \param captureType The type of capture backend. - * \param id The id of the camera. - * \param description A human readable description of the camera. - */ - CaptureDevice(const std::string captureType, const std::string id, const std::string description = ""); + /** + * \brief Constructor. + * + * \param captureType The type of capture backend. + * \param id The id of the camera. + * \param description A human readable description of the camera. + */ + CaptureDevice(const std::string captureType, const std::string id, + const std::string description = ""); - /** - * \brief Destructor. - */ - ~CaptureDevice(); + /** + * \brief Destructor. + */ + ~CaptureDevice(); - /** - * \brief The type of capture backend. - */ - std::string captureType() const; + /** + * \brief The type of capture backend. + */ + std::string captureType() const; - /** - * \brief The id of the camera. - */ - std::string id() const; + /** + * \brief The id of the camera. + */ + std::string id() const; - /** - * \brief The description of the camera. - */ - std::string description() const; + /** + * \brief The description of the camera. + */ + std::string description() const; - /** - * \brief A unique name consisting of the capture type and the id. - */ - std::string uniqueName() const; + /** + * \brief A unique name consisting of the capture type and the id. + */ + std::string uniqueName() const; private: - std::string mCaptureType; - std::string mId; - std::string mDescription; + std::string mCaptureType; + std::string mId; + std::string mDescription; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h b/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h index 2e4833a..28e3c49 100644 --- a/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h +++ b/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h @@ -38,101 +38,115 @@ #include "Platform.h" #include "Util.h" -namespace alvar { - +namespace alvar +{ class CaptureFactoryPrivate; /** * \brief CaptureFactory for creating Capture classes. * * CaptureFactory is a singleton that creates Capture classes used to perform - * camera acquisition. Different backends are implemented as dynamicly loaded plugins - * so that platform dependancies can be handled at runtime and not compile time. + * camera acquisition. Different backends are implemented as dynamicly loaded + * plugins so that platform dependancies can be handled at runtime and not + * compile time. */ class ALVAR_EXPORT CaptureFactory { public: - /** - * \brief The singleton instance of CaptureFactory. - */ - static CaptureFactory *instance(); - - /** - * \brief Vector of strings. - */ - typedef std::vector CapturePluginVector; - - /** - * \brief Enumerate capture plugins currently available. - * - * This method should be used carfully since it will load all the available plugins. - * - * \return A vector of strings identifying all currently available capture plugins. - */ - CapturePluginVector enumeratePlugins(); - - /** - * \brief Vector of CaptureDevices. - */ - typedef std::vector CaptureDeviceVector; - - /** - * \brief Enumerate capture devices currently available. - * - * This method should be used carfully since it will load all the known plugins - * and call their respective enumeration methods. The vector of CaptureDevice - * objects returned should be cached. - * - * \param captureType Force the enumeration of only one type of plugin. - * \return A vector of CaptureDevice objects that are currently available. - */ - CaptureDeviceVector enumerateDevices(const std::string &captureType = ""); - - /** - * \brief Create Capture class. Transfers onwership to the caller. - * - * If the needed backend plugin is not loaded, an attempt is made to load it and - * an instance of it is kept such that it is available for subsequent calls. - * - * \param captureDevice CaptureDevice object specifying the plugin to use. - * \return A new Capture class for which the caller takes ownership. - */ - Capture *createCapture(const CaptureDevice captureDevice); + /** + * \brief The singleton instance of CaptureFactory. + */ + static CaptureFactory* instance(); + + /** + * \brief Vector of strings. + */ + typedef std::vector CapturePluginVector; + + /** + * \brief Enumerate capture plugins currently available. + * + * This method should be used carfully since it will load all the available + * plugins. + * + * \return A vector of strings identifying all currently available capture + * plugins. + */ + CapturePluginVector enumeratePlugins(); + + /** + * \brief Vector of CaptureDevices. + */ + typedef std::vector CaptureDeviceVector; + + /** + * \brief Enumerate capture devices currently available. + * + * This method should be used carfully since it will load all the known + * plugins and call their respective enumeration methods. The vector of + * CaptureDevice objects returned should be cached. + * + * \param captureType Force the enumeration of only one type of plugin. + * \return A vector of CaptureDevice objects that are currently available. + */ + CaptureDeviceVector enumerateDevices(const std::string& captureType = ""); + + /** + * \brief Create Capture class. Transfers onwership to the caller. + * + * If the needed backend plugin is not loaded, an attempt is made to load it + * and an instance of it is kept such that it is available for subsequent + * calls. + * + * \param captureDevice CaptureDevice object specifying the plugin to use. + * \return A new Capture class for which the caller takes ownership. + */ + Capture* createCapture(const CaptureDevice captureDevice); protected: - /** - * \brief Destructor. - */ - ~CaptureFactory(); + /** + * \brief Destructor. + */ + ~CaptureFactory(); private: - /** - * \brief CaptureFactoryDestroyer for deleting the CaptureFactory singleton. - */ - class CaptureFactoryDestroyer + /** + * \brief CaptureFactoryDestroyer for deleting the CaptureFactory singleton. + */ + class CaptureFactoryDestroyer + { + public: + CaptureFactoryDestroyer(CaptureFactory* instance = NULL) + : mInstance(instance) + { + } + ~CaptureFactoryDestroyer() + { + delete mInstance; + } + void set(CaptureFactory* instance) { - public: - CaptureFactoryDestroyer(CaptureFactory *instance = NULL) : mInstance(instance) {} - ~CaptureFactoryDestroyer() {delete mInstance;} - void set(CaptureFactory *instance) {mInstance = instance;} - private: - CaptureFactory *mInstance; - }; - - // private constructors and assignment operator for singleton implementation - CaptureFactory(); - CaptureFactory(const CaptureFactory&); - CaptureFactory &operator=(const CaptureFactory&); - - // static members for singleton implementation - static CaptureFactory *mInstance; - static Mutex mMutex; - static CaptureFactoryDestroyer mDestroyer; - - // members - CaptureFactoryPrivate *d; + mInstance = instance; + } + + private: + CaptureFactory* mInstance; + }; + + // private constructors and assignment operator for singleton implementation + CaptureFactory(); + CaptureFactory(const CaptureFactory&); + CaptureFactory& operator=(const CaptureFactory&); + + // static members for singleton implementation + static CaptureFactory* mInstance; + static Mutex mMutex; + static CaptureFactoryDestroyer mDestroyer; + + // members + CaptureFactoryPrivate* d; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h b/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h index f3506d1..41f1470 100644 --- a/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h +++ b/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h @@ -30,38 +30,38 @@ #include "Plugin.h" -namespace alvar { - +namespace alvar +{ class CapturePlugin; class CaptureFactoryPrivate { public: - CaptureFactoryPrivate(); - ~CaptureFactoryPrivate(); + CaptureFactoryPrivate(); + ~CaptureFactoryPrivate(); - void setupPluginPaths(); - void parseEnvironmentVariable(const std::string &variable); - std::string pluginPrefix(); - std::string pluginExtension(); + void setupPluginPaths(); + void parseEnvironmentVariable(const std::string& variable); + std::string pluginPrefix(); + std::string pluginExtension(); - void loadPlugins(); - void loadPlugin(const std::string &captureType); - void loadPlugin(const std::string &captureType, const std::string &filename); - CapturePlugin *getPlugin(const std::string &captureType); + void loadPlugins(); + void loadPlugin(const std::string& captureType); + void loadPlugin(const std::string& captureType, const std::string& filename); + CapturePlugin* getPlugin(const std::string& captureType); - typedef std::vector PluginPathsVector; - PluginPathsVector mPluginPaths; - std::string mPluginPrefix; - std::string mPluginPostfix; + typedef std::vector PluginPathsVector; + PluginPathsVector mPluginPaths; + std::string mPluginPrefix; + std::string mPluginPostfix; - bool mLoadedAllPlugins; - typedef std::map PluginMap; - PluginMap mPluginMap; - typedef std::map CapturePluginMap; - CapturePluginMap mCapturePluginMap; + bool mLoadedAllPlugins; + typedef std::map PluginMap; + PluginMap mPluginMap; + typedef std::map CapturePluginMap; + CapturePluginMap mCapturePluginMap; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h b/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h index 3ba0ca3..b8ed826 100644 --- a/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h +++ b/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h @@ -33,56 +33,56 @@ #include "Alvar.h" #include "CaptureDevice.h" -namespace alvar { - +namespace alvar +{ /** * \brief CapturePlugin interface that plugins must implement. * - * All plugins must implement the CapturePlugin interface. When the plugin is loaded, - * the CapturePlugin implementation will register itself with the CaptureFactory. + * All plugins must implement the CapturePlugin interface. When the plugin is + * loaded, the CapturePlugin implementation will register itself with the + * CaptureFactory. */ class ALVAR_EXPORT CapturePlugin { public: - /** - * \brief Constructor. - * - * \param captureType A unique identifier for the capture plugin. - */ - CapturePlugin(const std::string &captureType) - : mCaptureType(captureType) - { - } + /** + * \brief Constructor. + * + * \param captureType A unique identifier for the capture plugin. + */ + CapturePlugin(const std::string& captureType) : mCaptureType(captureType) + { + } - /** - * \brief Destructor. - */ - virtual ~CapturePlugin() {}; + /** + * \brief Destructor. + */ + virtual ~CapturePlugin(){}; - /** - * \brief Vector of CaptureDevices. - */ - typedef std::vector CaptureDeviceVector; + /** + * \brief Vector of CaptureDevices. + */ + typedef std::vector CaptureDeviceVector; - /** - * \brief Enumerate capture devices currently available. - * - * \return A vector of CaptureDevice objects that are currently available. - */ - virtual CaptureDeviceVector enumerateDevices() = 0; + /** + * \brief Enumerate capture devices currently available. + * + * \return A vector of CaptureDevice objects that are currently available. + */ + virtual CaptureDeviceVector enumerateDevices() = 0; - /** - * \brief Create Capture class. Transfers onwership to the caller. - * - * \param captureDevice Information of which camera to create. - * \return A new Capture class for which the caller takes ownership. - */ - virtual Capture *createCapture(const CaptureDevice captureDevice) = 0; + /** + * \brief Create Capture class. Transfers onwership to the caller. + * + * \param captureDevice Information of which camera to create. + * \return A new Capture class for which the caller takes ownership. + */ + virtual Capture* createCapture(const CaptureDevice captureDevice) = 0; protected: - std::string mCaptureType; + std::string mCaptureType; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h b/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h index 8228e86..4a7fce7 100644 --- a/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h +++ b/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h @@ -35,105 +35,105 @@ #include "Line.h" #include "Camera.h" -namespace alvar { - +namespace alvar +{ /** * \brief Connected components labeling methods. -*/ + */ enum ALVAR_EXPORT LabelingMethod { - CVSEQ + CVSEQ }; /** * \brief Base class for labeling connected components from binary image. -*/ + */ class ALVAR_EXPORT Labeling { +protected: + Camera* cam; + int thresh_param1, thresh_param2; -protected : - - Camera *cam; - int thresh_param1, thresh_param2; - -public : - - /** - * \brief Pointer to grayscale image that is thresholded for labeling. - */ - IplImage *gray; - /** - * \brief Pointer to binary image that is then labeled. - */ - IplImage *bw; - - /** - * \brief Vector of 4-length vectors where the corners of detected blobs are stored. - */ - std::vector > blob_corners; - - /** - * \brief Two alternatives for thresholding the gray image. ADAPT (adaptive threshold) is only supported currently. - */ - enum ThresholdMethod - { - THRESH, - ADAPT - }; - - /** Constructor */ - Labeling(); - - /** Destructor*/ - virtual ~Labeling(); - - /** - * \brief Sets the camera object that is used to correct lens distortions. - */ - void SetCamera(Camera* camera) {cam = camera;} - - /** - * \brief Labels image and filters blobs to obtain square-shaped objects from the scene. - */ - virtual void LabelSquares(IplImage* image, bool visualize=false) = 0; - - bool CheckBorder(CvSeq* contour, int width, int height); - - void SetThreshParams(int param1, int param2) - { - thresh_param1 = param1; - thresh_param2 = param2; - } +public: + /** + * \brief Pointer to grayscale image that is thresholded for labeling. + */ + IplImage* gray; + /** + * \brief Pointer to binary image that is then labeled. + */ + IplImage* bw; + + /** + * \brief Vector of 4-length vectors where the corners of detected blobs are + * stored. + */ + std::vector > blob_corners; + + /** + * \brief Two alternatives for thresholding the gray image. ADAPT (adaptive + * threshold) is only supported currently. + */ + enum ThresholdMethod + { + THRESH, + ADAPT + }; + + /** Constructor */ + Labeling(); + + /** Destructor*/ + virtual ~Labeling(); + + /** + * \brief Sets the camera object that is used to correct lens distortions. + */ + void SetCamera(Camera* camera) + { + cam = camera; + } + + /** + * \brief Labels image and filters blobs to obtain square-shaped objects from + * the scene. + */ + virtual void LabelSquares(IplImage* image, bool visualize = false) = 0; + + bool CheckBorder(CvSeq* contour, int width, int height); + + void SetThreshParams(int param1, int param2) + { + thresh_param1 = param1; + thresh_param2 = param2; + } }; /** * \brief Labeling class that uses OpenCV routines to find connected components. -*/ + */ class ALVAR_EXPORT LabelingCvSeq : public Labeling { +protected: + int _n_blobs; + int _min_edge; + int _min_area; + bool detect_pose_grayscale; -protected : - - int _n_blobs; - int _min_edge; - int _min_area; - bool detect_pose_grayscale; - - CvMemStorage* storage; + CvMemStorage* storage; public: + LabelingCvSeq(); + ~LabelingCvSeq(); - LabelingCvSeq(); - ~LabelingCvSeq(); - - void SetOptions(bool _detect_pose_grayscale=false); + void SetOptions(bool _detect_pose_grayscale = false); - void LabelSquares(IplImage* image, bool visualize=false); + void LabelSquares(IplImage* image, bool visualize = false); - // TODO: Releases memory inside, cannot return CvSeq* - CvSeq* LabelImage(IplImage* image, int min_size, bool approx=false); + // TODO: Releases memory inside, cannot return CvSeq* + CvSeq* LabelImage(IplImage* image, int min_size, bool approx = false); }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Container3d.h b/ar_track_alvar/include/ar_track_alvar/Container3d.h index 2a24270..72a70d1 100644 --- a/ar_track_alvar/include/ar_track_alvar/Container3d.h +++ b/ar_track_alvar/include/ar_track_alvar/Container3d.h @@ -35,80 +35,107 @@ #include #include -namespace alvar { - -template class Container3d; +namespace alvar +{ +template +class Container3d; -/** \brief Functor class for \e Container3d \e Sort() to sort the search base using distance to specified origin. */ +/** \brief Functor class for \e Container3d \e Sort() to sort the search base + * using distance to specified origin. */ template -class Container3dSortDist { +class Container3dSortDist +{ protected: - CvPoint3D32f orig; - Container3d &container; + CvPoint3D32f orig; + Container3d& container; + public: - Container3dSortDist(Container3d &_container, const CvPoint3D32f _orig) : container(_container), orig(_orig) {} - bool operator()(size_t i1, size_t i2) - { - float x1 = container[i1].first.x-orig.x, x2 = container[i2].first.x-orig.x; - float y1 = container[i1].first.y-orig.y, y2 = container[i2].first.y-orig.y; - float z1 = container[i1].first.z-orig.z, z2 = container[i2].first.z-orig.z; - float d1 = x1*x1 + y1*y1 + z1*z1; - float d2 = x2*x2 + y2*y2 + z2*z2; - return d1& _container, const CvPoint3D32f _orig) + : container(_container), orig(_orig) + { + } + bool operator()(size_t i1, size_t i2) + { + float x1 = container[i1].first.x - orig.x, + x2 = container[i2].first.x - orig.x; + float y1 = container[i1].first.y - orig.y, + y2 = container[i2].first.y - orig.y; + float z1 = container[i1].first.z - orig.z, + z2 = container[i2].first.z - orig.z; + float d1 = x1 * x1 + y1 * y1 + z1 * z1; + float d2 = x2 * x2 + y2 * y2 + z2 * z2; + return d1 < d2; + } }; -/** \brief Functor class for \e Container3d \e Sort() to sort the search base using content size. */ +/** \brief Functor class for \e Container3d \e Sort() to sort the search base + * using content size. */ template -class Container3dSortSize { +class Container3dSortSize +{ protected: - Container3d &container; + Container3d& container; + public: - Container3dSortSize(Container3d &_container) : container(_container) {} - bool operator()(size_t i1, size_t i2) - { - return (container[i1].second->size() > container[i2].second->size()); - } + Container3dSortSize(Container3d& _container) : container(_container) + { + } + bool operator()(size_t i1, size_t i2) + { + return (container[i1].second->size() > container[i2].second->size()); + } }; -/** \brief Functor class for \e Container3d \e Limit() to limit the search space with distance */ +/** \brief Functor class for \e Container3d \e Limit() to limit the search space + * with distance */ template -class Container3dLimitDist { +class Container3dLimitDist +{ protected: - Container3d &container; - CvPoint3D32f orig; - float dist_limit; + Container3d& container; + CvPoint3D32f orig; + float dist_limit; + public: - Container3dLimitDist(Container3d &_container, const CvPoint3D32f _orig, float _dist_limit) - : container(_container),orig(_orig),dist_limit(_dist_limit) {} - bool operator()(size_t i) const { - float x = container[i].first.x-orig.x; - float y = container[i].first.y-orig.y; - float z = container[i].first.z-orig.z; - float d = x*x + y*y + z*z; - if (d <= dist_limit*dist_limit) return true; - return false; - } + Container3dLimitDist(Container3d& _container, const CvPoint3D32f _orig, + float _dist_limit) + : container(_container), orig(_orig), dist_limit(_dist_limit) + { + } + bool operator()(size_t i) const + { + float x = container[i].first.x - orig.x; + float y = container[i].first.y - orig.y; + float z = container[i].first.z - orig.z; + float d = x * x + y * y + z * z; + if (d <= dist_limit * dist_limit) + return true; + return false; + } }; /** - * \brief Generic container to store any information in 3D (features, photos, ...) + * \brief Generic container to store any information in 3D (features, photos, + * ...) * - * You can store any information in 3D using this container. Each element in the container has - * an unique id that it can be referenced with using \e operator[](). The indices are from 0 - * to \e size(). You can find specific index using \e GetIndex(Iterator &iter) or \e GetIndex(T *p) . + * You can store any information in 3D using this container. Each element in the + * container has an unique id that it can be referenced with using \e + * operator[](). The indices are from 0 to \e size(). You can find specific + * index using \e GetIndex(Iterator &iter) or \e GetIndex(T *p) . * - * In addition the Container3d contains also a 'search space' that can be iterated through - * using \e begin() and \e end(). This 'search space' can be limited using \e Limit() , - * sorted using \e Sort() and reseted using \e ResetSearchSpace(). You specify what to limit/sort - * using specified functors. In ALVAR there exists functors \e Container3dLimitDist , - * \e Container3dSortSize and \e Container3dSortDist . But you can quite well make your own - * versions (see example below). - * - * The implementation is optimized for a situation where there are a lot of searches between every - * time the search space is limited/ordered. This is the reason we use vector containers - * internally. The idea is that later we will improve the class by providing more ways for limiting - * and sorting the search space; for example Frustum culling. + * In addition the Container3d contains also a 'search space' that can be + * iterated through using \e begin() and \e end(). This 'search space' can be + * limited using \e Limit() , sorted using \e Sort() and reseted using \e + * ResetSearchSpace(). You specify what to limit/sort using specified functors. + * In ALVAR there exists functors \e Container3dLimitDist , \e + * Container3dSortSize and \e Container3dSortDist . But you can quite well make + * your own versions (see example below). + * + * The implementation is optimized for a situation where there are a lot of + * searches between every time the search space is limited/ordered. This is the + * reason we use vector containers internally. The idea is that later we will + * improve the class by providing more ways for limiting and sorting the search + * space; for example Frustum culling. * * Usage: * @@ -130,14 +157,14 @@ class Container3dLimitDist { * int x_min, x_max; * Container3d &container; * public: - * Container3dLimitX(Container3d &_container, int _x_min, int _x_max) + * Container3dLimitX(Container3d &_container, int _x_min, int _x_max) * : container(_container),x_min(_x_min),x_max(_x_max) {} * bool operator()(size_t i1) const { - * if ((container[i1].first.x >= x_min) && (container[i1].first.x <= x_max)) return true; - * return false; + * if ((container[i1].first.x >= x_min) && (container[i1].first.x <= x_max)) + * return true; return false; * } * }; - * + * * ... * Container3d c3d; * c3d.Add(CvPoint3D32f(0,0,0), 0); @@ -165,120 +192,175 @@ class Container3dLimitDist { template class Container3d { - public: - /** \brief \e node_type for storing data. 3D-position is paired with the data content. */ - typedef std::pair node_type; - protected: - /** \brief the actual data in using node_type: pair */ - std::vector data; - /** \brief Possibly limited set of indices for \e data in somehow "optimal" search order */ - std::vector search_space; +public: + /** \brief \e node_type for storing data. 3D-position is paired with the data + * content. */ + typedef std::pair node_type; + +protected: + /** \brief the actual data in using node_type: pair */ + std::vector data; + /** \brief Possibly limited set of indices for \e data in somehow "optimal" + * search order */ + std::vector search_space; + +public: + /** \brief Add \e _data in the container and associate it with 3D position \e + * _pos */ + void Add(const CvPoint3D32f& _pos, const T& _data) + { + data.push_back(node_type(_pos, _data)); + search_space.push_back(data.size() - 1); + } + /** \brief Clear the container */ + void Clear() + { + data.clear(); + search_space.clear(); + } + /** \brief Reset the search space to contain whole data */ + void ResetSearchSpace() + { + search_space.resize(data.size()); + for (size_t i = 0; i < search_space.size(); i++) + { + search_space[i] = i; + } + } + /** \brief Erase item in the container */ + void Erase(size_t index) + { + typename std::vector::iterator iter_d; + iter_d = data.begin(); + for (size_t i = 0; i < index; i++) + iter_d++; + data.erase(iter_d); + ResetSearchSpace(); + } + /** \brief Sort using external Compare method */ + template + int Sort(Compare comp) + { + stable_sort(search_space.begin(), search_space.end(), comp); + return search_space.size(); + } - public: - /** \brief Add \e _data in the container and associate it with 3D position \e _pos */ - void Add(const CvPoint3D32f& _pos, const T& _data){ - data.push_back(node_type(_pos, _data)); - search_space.push_back(data.size()-1); - } - /** \brief Clear the container */ - void Clear() { - data.clear(); - search_space.clear(); - } - /** \brief Reset the search space to contain whole data */ - void ResetSearchSpace() { - search_space.resize(data.size()); - for (size_t i=0; i::iterator iter_d; - iter_d = data.begin(); - for (size_t i=0; i - int Sort(Compare comp) { - stable_sort(search_space.begin(), search_space.end(), comp); - return search_space.size(); - } + /** \brief Limit the search space with external limitation */ + template + int Limit(Test test) + { + std::vector::iterator iter; + for (iter = search_space.begin(); iter != search_space.end();) + { + if (!test(*iter)) + { + iter = search_space.erase(iter); + } + else + { + iter++; + } + } + return search_space.size(); + } - /** \brief Limit the search space with external limitation */ - template - int Limit(Test test) { - std::vector::iterator iter; - for (iter = search_space.begin(); iter != search_space.end();) { - if (!test(*iter)) { - iter = search_space.erase(iter); - } else { - iter++; - } - } - return search_space.size(); - } + /** \brief Iterator for going through the items in \e Container3d in the + * specified order + * + * The idea is that the content in \e Container3d can be sorted and limited in + * different ways. After sorting/limiting the content the \e iterator (\e + * Begin() and \e End() ) can be used for accessing the data items in optimal + * order. + */ + class Iterator : public std::iterator + { + protected: + Container3d* container; + std::vector::iterator iter; - /** \brief Iterator for going through the items in \e Container3d in the specified order - * - * The idea is that the content in \e Container3d can be sorted and limited in different - * ways. After sorting/limiting the content the \e iterator (\e Begin() and \e End() ) can - * be used for accessing the data items in optimal order. - */ - class Iterator : public std::iterator - { - protected: - Container3d *container; - std::vector::iterator iter; - public: - Iterator() {} - Iterator(Container3d *_container, std::vector::iterator _iter) : container(_container), iter(_iter) {} - node_type &operator*() const { return container->data[*iter]; } - node_type *operator->() const { return &(operator*()); } - virtual Iterator& operator++() { ++iter; return *this; } - bool operator==(const Iterator& _m) const { return iter == _m.iter; } - bool operator!=(const Iterator& _m) const { return iter != _m.iter; } - size_t GetIndex() { return *iter; } - }; + public: + Iterator() + { + } + Iterator(Container3d* _container, std::vector::iterator _iter) + : container(_container), iter(_iter) + { + } + node_type& operator*() const + { + return container->data[*iter]; + } + node_type* operator->() const + { + return &(operator*()); + } + virtual Iterator& operator++() + { + ++iter; + return *this; + } + bool operator==(const Iterator& _m) const + { + return iter == _m.iter; + } + bool operator!=(const Iterator& _m) const + { + return iter != _m.iter; + } + size_t GetIndex() + { + return *iter; + } + }; - /** \brief Provides an iterator pointing to the beginning of the limited/sorted 3D content */ - Iterator begin() { - return Iterator(this, search_space.begin()); - } + /** \brief Provides an iterator pointing to the beginning of the + * limited/sorted 3D content */ + Iterator begin() + { + return Iterator(this, search_space.begin()); + } - /** \brief Provides an iterator pointing to the end of the limited/sorted 3D content */ - Iterator end() { - return Iterator(this, search_space.end()); - } + /** \brief Provides an iterator pointing to the end of the limited/sorted 3D + * content */ + Iterator end() + { + return Iterator(this, search_space.end()); + } - /** \brief Get number of items that can be referenced using \e operator[]() */ - size_t size() const { return data.size(); } + /** \brief Get number of items that can be referenced using \e operator[]() */ + size_t size() const + { + return data.size(); + } - /** \brief Get absolute reference usable with \e operator[]() based on the iterator */ - size_t GetIndex(Iterator &iter) { - return iter.GetIndex(); - } + /** \brief Get absolute reference usable with \e operator[]() based on the + * iterator */ + size_t GetIndex(Iterator& iter) + { + return iter.GetIndex(); + } - /** \brief Instead of \e Iterator we can use also absolute references for data with \e operator[]() */ - node_type &operator[](size_t index) { - return data[index]; - } + /** \brief Instead of \e Iterator we can use also absolute references for data + * with \e operator[]() */ + node_type& operator[](size_t index) + { + return data[index]; + } - /** \brief Get absolute reference usable with \e operator[]() based on the content */ - size_t GetIndex(T *p) { - size_t i=0; - for (; i