MultiMarkerBundle.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 "ar_track_alvar/MultiMarkerBundle.h"
00025 
00026 using namespace std;
00027 
00028 namespace alvar {
00029 using namespace std;
00030 
00031 MultiMarkerBundle::MultiMarkerBundle(std::vector<int>& indices) 
00032         : MultiMarker(indices)
00033 {
00034         MeasurementsReset();
00035 }
00036 
00037 MultiMarkerBundle::~MultiMarkerBundle()
00038 {
00039 }
00040 
00041 void MultiMarkerBundle::MeasurementsReset() {
00042         optimization_error=-1;
00043         optimization_keyframes=0;
00044         optimization_markers=0;
00045         optimizing=false;
00046         camera_poses.clear();
00047         measurements.clear();
00048 }
00049 
00050 int n_images; // TODO: This should not be global (use the param instead)
00051 int n_markers; // TODO: This should not be global (use the param instead)
00052 Camera *camera; // TODO: This should not be global (use the param instead)
00053 
00054 void Est(CvMat* state, CvMat* estimation, void *param)
00055 {
00056 
00057         // State: cam1, cam2, cam3, cam4, ..., X1(x,y,z), X2, X3, ...
00058         // Estimation: (u11,v11), (u)
00059 
00060         // For every image observation (camera)...
00061         for(int i = 0; i < n_images; ++i)
00062         {
00063                 // Get camera from state
00064                 Pose p; p.SetQuaternion(&(state->data.db[i*7+3]));
00065 
00066                 double tra[3];
00067                 double rodr[3];
00068                 CvMat mat_translation_vector = cvMat(3, 1, CV_64F, tra);
00069                 CvMat mat_rotation_vector = cvMat(3, 1, CV_64F, rodr);
00070 
00071                 memcpy(tra, &(state->data.db[i*7]), 3*sizeof(double));
00072                 p.GetRodriques(&mat_rotation_vector);
00073 
00074                 // For every point in marker field
00075                 int n_points = n_markers*4;
00076                 for(int j = 0; j < n_points; ++j)
00077                 {
00078                         int index = n_images*7 + 3*j;
00079 
00080                         double object_points[3] = {state->data.db[index+0],
00081                                                                            state->data.db[index+1],
00082                                                                            state->data.db[index+2]};
00083 
00084 
00085                         CvMat mat_object_points;
00086                         cvInitMatHeader(&mat_object_points, 1, 1, CV_64FC3, object_points);
00087 
00088                         double proj[2]={0};
00089                         CvMat mat_proj = cvMat(1, 1, CV_64FC2, proj);
00090 
00091                         cvProjectPoints2(&mat_object_points, &mat_rotation_vector,
00092                                 &mat_translation_vector, &(camera->calib_K),
00093                                 &(camera->calib_D), &mat_proj);
00094 
00095                         index = i*n_points*2 + j*2;
00096                         estimation->data.db[index+0] = proj[0];
00097                         estimation->data.db[index+1] = proj[1];
00098                 }
00099         }
00100 }
00101 
00102 bool MultiMarkerBundle::Optimize(Camera *_cam, double stop, int max_iter, Optimization::OptimizeMethod method)
00103 {
00104         // Est() needs these
00105         // TODO: Better way to deliver them??? Other than using global variables???
00106         camera    = _cam; 
00107         n_images  = camera_poses.size();
00108         n_markers = marker_indices.size();
00109 
00110         if(n_images < 1)
00111         {
00112                 cout<<"Too few images! At least 1 images needed."<<endl;
00113                 return false;
00114         }
00115 
00116         optimizing = true;
00117 
00118         // Initialize
00119         size_t frames = camera_poses.size();
00120         int n_params = frames*7 + 3*4*n_markers;
00121         int n_meas = 2*4*n_markers*frames;
00122         CvMat* parameters_mat   = cvCreateMat(n_params, 1, CV_64F);
00123         CvMat* parameters_mask_mat = cvCreateMat(n_params, 1, CV_8U);
00124         CvMat* measurements_mat = cvCreateMat(n_meas, 1, CV_64F);
00125         CvMat* weight_mat = cvCreateMat(n_meas, 1, CV_64F);
00126         cvZero(parameters_mat);
00127         cvSet(parameters_mask_mat, cvScalar(1));
00128         cvZero(measurements_mat);
00129         cvSet(weight_mat, cvRealScalar(1.0));
00130 
00131         // Fill in the point cloud that is used as starting point for optimization
00132         for(size_t i = 0; i < marker_indices.size(); ++i) {
00133                 int id = marker_indices[i];
00134                 for (int j=0; j<4; j++) {
00135                         //hop int index = frames*7 + id*(3*4) + j*3;
00136                         int index = frames*7 + i*(3*4) + j*3;
00137                         // Lets keep the base marker constant (1st marker given in the indices list)
00138                         if (i == 0) {
00139                                 cvSet2D(parameters_mask_mat, index+0, 0, cvScalar(0));
00140                                 cvSet2D(parameters_mask_mat, index+1, 0, cvScalar(0));
00141                                 cvSet2D(parameters_mask_mat, index+2, 0, cvScalar(0));
00142                         }
00143                         if (marker_status[i] > 0) {
00144                                 cvmSet(parameters_mat, index+0, 0, pointcloud[pointcloud_index(id,j)].x);
00145                                 cvmSet(parameters_mat, index+1, 0, pointcloud[pointcloud_index(id,j)].y);
00146                                 cvmSet(parameters_mat, index+2, 0, pointcloud[pointcloud_index(id,j)].z);
00147                         } else {
00148                                 // We don't optimize known-initialized parameters?
00149                                 cvSet2D(parameters_mask_mat, index+0, 0, cvScalar(0));
00150                                 cvSet2D(parameters_mask_mat, index+1, 0, cvScalar(0));
00151                                 cvSet2D(parameters_mask_mat, index+2, 0, cvScalar(0));
00152                         }
00153                 }
00154         }
00155         // Fill in the frame data. Camera poses into parameters and corners into measurements
00156         int n_measurements = 0; // number of actual measurement data points.
00157         for (size_t f=0; f < frames; f++) {
00158                 //cout<<"frame "<<f<<" / "<<frames<<endl;
00159                 // Camera pose
00160                 CvMat tra  = cvMat(3, 1, CV_64F, &(parameters_mat->data.db[f*7+0]));
00161                 CvMat qua  = cvMat(4, 1, CV_64F, &(parameters_mat->data.db[f*7+3]));
00162                 camera_poses[f].GetTranslation(&tra);
00163                 camera_poses[f].GetQuaternion(&qua);
00164                 // Measurements
00165                 for(size_t i = 0; i < marker_indices.size(); ++i) {
00166                         int id = marker_indices[i];
00167                         if (measurements.find(measurements_index(f,id,0)) != measurements.end()) {
00168                                 for (int j=0; j<4; j++) {
00169                                         //cout<<measurements[measurements_index(f, id, j)].x<<endl;
00170                                         //hop int index = f*(n_markers*4*2) + id*(4*2) + j*2;
00171                                         int index = f*(n_markers*4*2) + i*(4*2) + j*2;
00172                                         cvmSet(measurements_mat, index+0, 0, measurements[measurements_index(f, id, j)].x);
00173                                         cvmSet(measurements_mat, index+1, 0, measurements[measurements_index(f, id, j)].y);
00174                                         n_measurements += 2;
00175                                 }
00176                         } else {
00177                                 for (int j=0; j<4; j++) {
00178                                         //hop int index = f*(n_markers*4*2) + id*(4*2) + j*2;
00179                                         int index = f*(n_markers*4*2) + i*(4*2) + j*2;
00180                                         cvmSet(weight_mat, index+0, 0, 0.);
00181                                         cvmSet(weight_mat, index+1, 0, 0.);
00182                                 }
00183                         }
00184                 }
00185         }
00186         //out_matrix(measurements_mat, "measurements");
00187         //out_matrix(parameters_mat, "parameters");
00188         optimization_keyframes = n_images;
00189         optimization_markers = 0;
00190         for(size_t i = 0; i < marker_indices.size(); ++i) if (marker_status[i] > 0) optimization_markers++;
00191         Optimization optimization(n_params, n_meas);
00192         cout<<"Optimizing with "<<optimization_keyframes<<" keyframes and "<<optimization_markers<<" markers"<<endl;
00193         optimization_error = 
00194                 optimization.Optimize(parameters_mat, measurements_mat, stop, max_iter, 
00195                                       Est, 0, method, parameters_mask_mat, NULL, weight_mat);
00196         optimization_error /= n_measurements;
00197         cout<<"Optimization error per corner: "<<optimization_error<<endl;
00198         /*
00199         if ((frames > 3) && (optimization_error > stop)) {
00200                 CvMat *err = optimization.GetErr();
00201                 int max_k=-1;
00202                 double max=0;
00203                 for (int k=0; k<err->height; k++) {
00204                         if (cvmGet(err, k, 0) > max) {
00205                                 max = cvmGet(err, k, 0);
00206                                 max_k = k;
00207                         }
00208                 }
00209                 if (max_k >= 0) {
00210                         // We remove all 8 corner measurements
00211                         int f = (max_k - (max_k % (n_markers*4*2))) / (n_markers*4*2);
00212                         max_k -= f*(n_markers*4*2);
00213                         int id = (max_k - (max_k % (4*2))) / (4*2);
00214                         cout<<"Optimization error over the threshold -- remove the biggest outlier: ";
00215                         cout<<"frame "<<f<<" id "<<id<<endl;
00216                         measurements.erase(measurements_index(f,id,0));
00217                         measurements.erase(measurements_index(f,id,1));
00218                         measurements.erase(measurements_index(f,id,2));
00219                         measurements.erase(measurements_index(f,id,3));
00220                         for (int j=0; j<4; j++) {
00221                                 int index = f*(n_markers*4*2) + id*(4*2) + j*2;
00222                                 cvmSet(measurements_mat, index+0, 0, measurements[measurements_index(f, id, j)].x);
00223                                 cvmSet(measurements_mat, index+1, 0, measurements[measurements_index(f, id, j)].y);
00224                         }
00225                         optimization_error = optimization.Optimize(parameters_mat, measurements_mat, stop, max_iter, Est, method, parameters_mask_mat);
00226                         cout<<"Optimization error: "<<optimization_error<<endl;
00227                 }
00228         }
00229         */
00230         //out_matrix(parameters_mat, "parameters");
00231         //out_matrix(parameters_mask_mat, "parameters_mask");
00232 
00233         // Fill in the point cloud with optimized values
00234         for(size_t i = 0; i < marker_indices.size(); ++i) {
00235                 int id = marker_indices[i];
00236                 for (int j=0; j<4; j++) {
00237                         //hop int index = frames*7 + id*(3*4) + j*3;
00238                         int index = frames*7 + i*(3*4) + j*3;
00239                         pointcloud[pointcloud_index(id,j)].x = cvmGet(parameters_mat, index+0,0);
00240                         pointcloud[pointcloud_index(id,j)].y = cvmGet(parameters_mat, index+1,0);
00241                         pointcloud[pointcloud_index(id,j)].z = cvmGet(parameters_mat, index+2,0);
00242                 }
00243         }
00244 
00245         cvReleaseMat(&parameters_mat);
00246         cvReleaseMat(&parameters_mask_mat);
00247         cvReleaseMat(&measurements_mat);
00248 
00249         optimizing = false;
00250         return true;    
00251 }
00252 
00253 void MultiMarkerBundle::_MeasurementsAdd(MarkerIterator &begin, MarkerIterator &end, const Pose& camera_pose) {
00254         camera_poses.push_back(camera_pose);
00255         int frame_no = camera_poses.size()-1;
00256         //cout<<"Adding measurements for frame "<<frame_no<<endl;
00257   for (MarkerIterator &i = begin.reset(); i != end; ++i)
00258         {
00259                 const Marker* marker = *i;
00260                 int id = marker->GetId();
00261                 int index = get_id_index(id);
00262                 if (index < 0) continue;
00263                 //cout<<"Id "<<id<<" ";
00264                 for (int j = 0; j<4; j++) {
00265                         measurements[measurements_index(frame_no, id, j)] = 
00266                                 marker->marker_corners_img[j];
00267                         //cout<<markers->at(i).marker_corners_img[j].x<<" ";
00268                 }
00269                 //cout<<endl;
00270         }
00271 }
00272 
00273 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02