MultiMarkerInitializer.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "MultiMarkerInitializer.h"
00025 
00026 using namespace std;
00027 
00028 namespace alvar {
00029 using namespace std;
00030 
00031 MultiMarkerInitializer::MultiMarkerInitializer(std::vector<int>& indices, int _filter_buffer_min, int _filter_buffer_max)
00032         : MultiMarker(indices), filter_buffer_min(_filter_buffer_min) {
00033 
00034         marker_detected.resize(indices.size());
00035         pointcloud_filtered = new FilterMedian[indices.size()*4*3];
00036         for (size_t i=0; i<indices.size()*4*3; i++) {
00037                 pointcloud_filtered[i].setWindowSize(_filter_buffer_max);
00038         }
00039 
00040         MeasurementsReset();
00041 }
00042 
00043 void MultiMarkerInitializer::MeasurementsAdd(MarkerIterator &begin, MarkerIterator &end) {
00044         // copy markers into measurements.
00045         vector<MarkerMeasurement> new_measurements;
00046         for (MarkerIterator &i = begin.reset(); i != end; ++i) {
00047                 const Marker* marker = *i;
00048                 int index = get_id_index(marker->GetId()); 
00049                 if (index == -1) continue;
00050                 MarkerMeasurement m;
00051                 m.SetId(marker->GetId());
00052                 m.SetMarkerSize(marker->GetMarkerEdgeLength(), marker->GetRes(), marker->GetMargin());
00053                 m.pose = marker->pose;
00054                 m.marker_corners_img = i->marker_corners_img;
00055                 new_measurements.push_back(m);
00056                 marker_detected[index] = true;
00057         }
00058 
00059         // If we have seen the 0 marker the first time we push it into point could.
00060         for (MarkerIterator &i = begin.reset(); i != end; ++i) {
00061                 const Marker* marker = *i;
00062                 int id = marker->GetId();
00063                 int index = get_id_index(id);
00064                 // Initialize marker pose
00065                 //if (id == 0 && marker_status[index] == 0)
00066                 if (index == 0 && marker_status[index] == 0)
00067                 {
00068                         Pose pose;
00069                         CvPoint3D64f corners[4];
00070                         PointCloudCorners3d(marker->GetMarkerEdgeLength(), pose, corners);
00071                         for(size_t j = 0; j < 4; ++j) {
00072                                 int p_index = pointcloud_index(id, j);
00073                                 pointcloud[p_index] = corners[j];
00074                         }
00075                         marker_status[index] = 1;
00076                 }
00077         }
00078 
00079         measurements.push_back(new_measurements);
00080 }
00081 
00082 void MultiMarkerInitializer::MeasurementsReset() {
00083         measurements.clear();
00084         PointCloudReset();
00085         fill(marker_status.begin(), marker_status.end(), 0);
00086         fill(marker_detected.begin(), marker_detected.end(), false);
00087 
00088         for (size_t i=0; i<marker_indices.size()*4*3; i++) {
00089                 pointcloud_filtered[i].reset();
00090         }
00091 }
00092 
00093 int MultiMarkerInitializer::Initialize(Camera* cam) {
00094         for (bool found_new = true; found_new; ) {
00095                 found_new = false;
00096                 // Iterate through all measurements, try to compute Pose for each.
00097                 for (MeasurementIterator mi = measurements.begin(); mi != measurements.end(); ++mi) {
00098                         vector<MarkerMeasurement> &markers = *mi;
00099                         Pose pose;
00100                         MarkerIteratorImpl<MarkerMeasurement> m_begin(markers.begin());
00101                         MarkerIteratorImpl<MarkerMeasurement> m_end(markers.end());
00102                         double err = _GetPose(m_begin, m_end, cam, pose, NULL);
00103                         if (err >= 0) {
00104                                 // If pose is found, estimate marker poses for those that are still unkown.
00105                                 found_new = updateMarkerPoses(markers, pose);
00106                         }
00107                 }
00108         }
00109 
00110         // Check if every marker has been seen
00111         int n_detected = 0;
00112         for (unsigned int i = 0; i < marker_indices.size(); ++i) {
00113                 cout << i << " " << marker_detected[i] << " " << marker_status[i] << "\n";
00114                 if (marker_detected[i] && marker_status[i] != 0) ++n_detected;
00115         }
00116         return n_detected;
00117 }
00118 
00119 bool MultiMarkerInitializer::updateMarkerPoses(vector<MarkerMeasurement> &markers, const Pose &pose) {
00120         bool found_new = false;
00121         for (vector<MarkerMeasurement>::iterator i = markers.begin(); i != markers.end(); ++i) {
00122                 MarkerMeasurement &marker = *i;
00123                 int id = marker.GetId();
00124                 int index = get_id_index(id);
00125                 if (index > 0 && !marker.globalPose) {
00126                         // Compute absolute marker pose.
00127                         double cam_posed[16];
00128                         double mar_posed[16];
00129                         CvMat cam_mat = cvMat(4, 4, CV_64F, cam_posed);
00130                         CvMat mar_mat = cvMat(4, 4, CV_64F, mar_posed);
00131                         pose.GetMatrix(&cam_mat);
00132                         marker.pose.GetMatrix(&mar_mat);
00133                         cvInvert(&cam_mat, &cam_mat);
00134                         cvMatMul(&cam_mat, &mar_mat, &mar_mat);
00135                         marker.pose.SetMatrix(&mar_mat);
00136                         // Put marker into point cloud
00137                         CvPoint3D64f corners[4];
00138                         PointCloudCorners3d(marker.GetMarkerEdgeLength(), marker.pose, corners);
00139                         for(size_t j = 0; j < 4; ++j) {
00140                                 CvPoint3D64f p;
00141                                 int p_index = pointcloud_index(id, j);
00142                                 p.x = pointcloud_filtered[3*p_index+0].next(corners[j].x);
00143                                 p.y = pointcloud_filtered[3*p_index+1].next(corners[j].y);
00144                                 p.z = pointcloud_filtered[3*p_index+2].next(corners[j].z);
00145                                 if (pointcloud_filtered[3*p_index+0].getCurrentSize() >= filter_buffer_min) {
00146                                         pointcloud[p_index] = p;
00147                                         marker_status[index] = 1;
00148                                 }
00149                         }
00150                         marker.globalPose = 1;
00151                         found_new = true;
00152                 }
00153         }
00154         return found_new;
00155 }
00156 
00157 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sat Dec 28 2013 16:46:15