Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "MultiMarkerFiltered.h"
00025
00026 using namespace std;
00027
00028 namespace alvar {
00029 using namespace std;
00030
00031 MultiMarkerFiltered::MultiMarkerFiltered(std::vector<int>& indices)
00032 : MultiMarker(indices)
00033 {
00034 pointcloud_filtered = new FilterMedian[indices.size()*4*3];
00035 for (size_t i=0; i<indices.size()*4*3; i++) {
00036 pointcloud_filtered[i].setWindowSize(filter_buffer_max);
00037 }
00038 }
00039
00040 MultiMarkerFiltered::~MultiMarkerFiltered()
00041 {
00042 delete [] pointcloud_filtered;
00043 }
00044
00045 void MultiMarkerFiltered::PointCloudAverage(int marker_id, double edge_length, Pose &pose) {
00046 if (marker_id == 0) {
00047 if (marker_status[get_id_index(marker_id)] == 0) PointCloudAdd(marker_id, edge_length, pose);
00048 } else {
00049 CvPoint3D64f corners[4];
00050 PointCloudCorners3d(edge_length, pose, corners);
00051 for(size_t j = 0; j < 4; ++j) {
00052 int id_index = get_id_index(marker_id);
00053 if (id_index < 0) continue;
00054 int index = id_index*4*3 + j*3;
00055 CvPoint3D64f p;
00056 p.x = pointcloud_filtered[index+0].next(corners[j].x);
00057 p.y = pointcloud_filtered[index+1].next(corners[j].y);
00058 p.z = pointcloud_filtered[index+2].next(corners[j].z);
00059 pointcloud[pointcloud_index(marker_id, j)] = p;
00060
00061 if (pointcloud_filtered[index+0].getCurrentSize() >= filter_buffer_max) {
00062 marker_status[get_id_index(marker_id)]=1;
00063 }
00064 }
00065 }
00066 }
00067
00068 double MultiMarkerFiltered::_Update(MarkerIterator &begin, MarkerIterator &end,
00069 Camera* cam, Pose& pose, IplImage* image)
00070 {
00071 if (_GetPose(begin, end, cam, pose, NULL) == -1) return -1;
00072
00073
00074 for (MarkerIterator &i = begin.reset(); i != end; ++i)
00075 {
00076 const Marker* marker = *i;
00077 int id = marker->GetId();
00078 int index = get_id_index(id);
00079 if (index < 0) continue;
00080
00081
00082 if (marker_status[index] == 0)
00083 {
00084 double cam_posed[16];
00085 double mar_posed[16];
00086
00087 CvMat cam_mat = cvMat(4, 4, CV_64F, cam_posed);
00088 CvMat mar_mat = cvMat(4, 4, CV_64F, mar_posed);
00089
00090 pose.GetMatrix(&cam_mat);
00091 marker->pose.GetMatrix(&mar_mat);
00092
00093 cvInvert(&cam_mat, &cam_mat);
00094 cvMatMul(&cam_mat, &mar_mat, &mar_mat);
00095
00096 Pose p;
00097 p.SetMatrix(&mar_mat);
00098 PointCloudAverage(id, marker->GetMarkerEdgeLength(), p);
00099 }
00100 }
00101
00102
00103 return _GetPose(begin, end, cam, pose, image);
00104 }
00105
00106 }