double MultiMarkerFiltered::_Update(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image) { if (_GetPose(begin, end, cam, pose, NULL) == -1) return -1; // For every detected marker for (MarkerIterator &i = begin.reset(); i != end; ++i) { const Marker* marker = *i; int id = marker->GetId(); int index = get_id_index(id); if (index < 0) continue; // Initialize marker pose if (marker_status[index] == 0) { double cam_posed[16]; double mar_posed[16]; CvMat cam_mat = cvMat(4, 4, CV_64F, cam_posed); CvMat mar_mat = cvMat(4, 4, CV_64F, mar_posed); pose.GetMatrix(&cam_mat); marker->pose.GetMatrix(&mar_mat); cvInvert(&cam_mat, &cam_mat); cvMatMul(&cam_mat, &mar_mat, &mar_mat); Pose p; p.SetMatrix(&mar_mat); PointCloudAverage(id, marker->GetMarkerEdgeLength(), p); } } // When the pointcloud is updated we will get the new "better" pose... return _GetPose(begin, end, cam, pose, image); }