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 }
