tracker.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <image_transport/image_transport.h>
00003 #include <opencv/cv.h>
00004 #include <cv_bridge/CvBridge.h>
00005 #include <LinearMath/btVector3.h>
00006 #include <LinearMath/btQuaternion.h>
00007 
00008 #include <sensor_msgs/Image.h>
00009 #include <sensor_msgs/CameraInfo.h>
00010 
00011 #include "aruco/aruco.h"
00012 
00013 #include <iostream>
00014 #include <XmlRpcValue.h>
00015 
00016 #include "PoseEstimator.h"
00017 #include "CameraModelPointCorrector.h"
00018 #include "MarkerRefiner.h"
00019 #include "MarkerPublisher.h"
00020 #include "TransformPublisher.h"
00021 
00022 using namespace std;
00023 using namespace XmlRpc;
00024 using namespace ros;
00025 using namespace image_transport;
00026 using namespace sensor_msgs;
00027 using namespace aruco;
00028 using namespace EdgeDetection;
00029 
00030 PoseEstimator* poseEstimator;
00031 CameraModelPointCorrector* cameraModelPointCorrector;
00032 MarkerRefiner* markerRefiner;
00033 MarkerPublisher* markerPublisher;
00034 TransformPublisher* transformPublisher;
00035 
00036 void handleImage(const ImageConstPtr& image, const CameraInfoConstPtr& cameraInfo)
00037 {
00038         CvBridge bridge;
00039         IplImage* iplImage = bridge.imgMsgToCv(image, "rgb8");
00040         Mat matrixImage = iplImage;
00041 
00042         cameraModelPointCorrector->updateCameraModel(cameraInfo);
00043 
00044         double intrinsicCameraMatrix[9];
00045         for (int index = 0; index < 9; index++) intrinsicCameraMatrix[index] = cameraInfo->K[index];
00046 
00047         vector<aruco::Marker> markers;
00048 
00049         MarkerDetector markerDetector;
00050         markerDetector.detect(matrixImage, markers, CameraParameters());
00051 
00052         for (vector<aruco::Marker>::iterator marker = markers.begin(); marker < markers.end(); marker++)
00053         {
00054                 aruco::Marker refinedMarker = markerRefiner->refineMarker(*marker, matrixImage);
00055 
00056                 btVector3 position;
00057                 btQuaternion orientation;
00058                 if (poseEstimator->estimatePose(refinedMarker, intrinsicCameraMatrix, position, orientation))
00059                 {
00060                         if (markerPublisher != 0) markerPublisher->publishMarker(image->header, refinedMarker, position, orientation);
00061                         if (transformPublisher != 0) transformPublisher->publishTransform(image->header, refinedMarker, position, orientation);
00062                 }
00063         }
00064 }
00065 
00066 int main(int argc, char** argv)
00067 {
00068         init(argc, argv, "aruco_pose");
00069 
00070         NodeHandle nodeHandle("~");
00071 
00072         string correctionTablePath;
00073         nodeHandle.param("correction_table_path", correctionTablePath, string("MicroEdgeCorrectionTable.bin"));
00074         double markerSize;
00075         nodeHandle.param("marker_size", markerSize, 1.0);
00076         bool publishMarkers;
00077         nodeHandle.param("publish_markers", publishMarkers, false);
00078         bool publishTransforms;
00079         nodeHandle.param("publish_transforms", publishTransforms, true);
00080 
00081         ImageTransport imageTransport(nodeHandle);
00082         imageTransport.subscribeCamera("/image", 1, handleImage);
00083 
00084         poseEstimator = new PoseEstimator(markerSize);
00085         cameraModelPointCorrector = new CameraModelPointCorrector();
00086         markerRefiner = new MarkerRefiner(correctionTablePath, cameraModelPointCorrector);
00087         if (publishMarkers) markerPublisher = new MarkerPublisher(nodeHandle, markerSize);
00088         if (publishTransforms) transformPublisher = new TransformPublisher();
00089 
00090         spin();
00091 
00092         delete transformPublisher;
00093         delete markerPublisher;
00094         delete markerRefiner;
00095         delete cameraModelPointCorrector;
00096         delete poseEstimator;
00097 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


aruco_pose
Author(s): Julian Brunner
autogenerated on Thu May 23 2013 12:15:46