SfM.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 "SfM.h"
00025 
00026 namespace alvar {
00027 
00028 class CameraMoves
00029 {
00030         bool is_initialized;
00031         double prev[3];
00032 
00033 public:
00034 
00035         CameraMoves()
00036         {
00037                 is_initialized = false;
00038                 memset(prev, 0, 3*sizeof(double));
00039         }
00040 
00041         bool UpdateDistance(Pose* pose, double limit=10)
00042         {
00043                 double trad[3]; CvMat tra = cvMat(3, 1, CV_64F, trad);
00044                 Pose p = *pose;
00045                 p.Invert();
00046                 p.GetTranslation(&tra);
00047                 if(is_initialized == false)
00048                 {
00049                         memcpy(prev, trad, 3*sizeof(double));
00050                         is_initialized = true;
00051                         return false;
00052                 }
00053                 double dist = (prev[0]-trad[0])*(prev[0]-trad[0]) + 
00054                                                 (prev[1]-trad[1])*(prev[1]-trad[1]) + 
00055                                                 (prev[2]-trad[2])*(prev[2]-trad[2]);
00056                 if(dist > limit)
00057                 {
00058                         memcpy(prev, trad, 3*sizeof(double));
00059                         return true;
00060                 }
00061                 return false;           
00062         }
00063 };
00064 CameraMoves moving;
00065 
00066 void SimpleSfM::Reset(bool reset_also_triangulated) {
00067         pose_ok = false;
00068         container = container_reset_point;
00069         if (reset_also_triangulated) {
00070                 container_triangulated = container_triangulated_reset_point;
00071         }
00072         pose_difference.Reset();
00073         pose_difference.Mirror(false, true, true);
00074 }
00075 
00076 void SimpleSfM::SetResetPoint() {
00077         container_reset_point = container;
00078         container_triangulated_reset_point = container_triangulated;
00079 }
00080 
00081 void SimpleSfM::Clear() {
00082         container.clear();
00083 }
00084 
00085 CameraEC *SimpleSfM::GetCamera() { return &cam; }
00086 
00087 Pose *SimpleSfM::GetPose() { return &pose; }
00088 
00089 bool SimpleSfM::AddMultiMarker(const char *fname, FILE_FORMAT format/* = FILE_FORMAT_XML*/) {
00090         MultiMarkerEC mm;
00091         return mm.Load(container, fname, format, 0, 0,1023);
00092 }
00093 
00094 bool SimpleSfM::AddMultiMarker(MultiMarkerEC *mm) {
00095         return mm->MarkersToEC(container, 0, 0, 1023);
00096 }
00097 
00098 void SimpleSfM::AddMarker(int marker_id, double edge_length, Pose &pose) {
00099         marker_detector.MarkerToEC(container, marker_id, edge_length, pose, 0, 0, 1023);
00100 }
00101 
00102 float PointVectorFromCamera(CvPoint3D32f p3d, CvPoint3D32f &p3d_vec, Pose *camera_pose) {
00103         double pd[16], v[4] = {0,0,0,1};        
00104         CvMat Pi = cvMat(4, 4, CV_64F, pd);
00105         CvMat V = cvMat(4, 1, CV_64F, v);
00106 
00107         // Camera location in marker coordinates
00108         camera_pose->GetMatrix(&Pi);
00109         cvInv(&Pi, &Pi);
00110         cvMatMul(&Pi, &V, &V);
00111         v[0] /= v[3];
00112         v[1] /= v[3];
00113         v[2] /= v[3];
00114 
00115         // Vector from camera-point to the 3D-point in marker coordinates
00116         p3d_vec.x = float(p3d.x - v[0]);
00117         p3d_vec.y = float(p3d.y - v[1]);
00118         p3d_vec.z = float(p3d.z - v[2]);
00119 
00120         return std::sqrt(p3d_vec.x*p3d_vec.x + p3d_vec.y*p3d_vec.y + p3d_vec.z*p3d_vec.z);
00121 }
00122 
00123 void CreateShadowPoint(CvPoint3D32f &p3d_sh, CvPoint3D32f p3d, CameraEC *cam, Pose *camera_pose, float parallax_length, float triangulate_angle) {
00124         // Vector from camera-point to the 3D-point in marker coordinates
00125         float l = PointVectorFromCamera(p3d, p3d_sh, camera_pose);
00126 
00127         // Figure out a suitable matching shadow point distance
00128         CvPoint2D32f p2d;
00129         CvPoint3D32f p3d2;
00130         cam->ProjectPoint(p3d, camera_pose, p2d);
00131         p2d.x += parallax_length;
00132         cam->Get3dOnDepth(camera_pose, p2d, l, p3d2);
00133         p3d2.x -= p3d.x;
00134         p3d2.y -= p3d.y;
00135         p3d2.z -= p3d.z;
00136         float pl = std::sqrt(p3d2.x*p3d2.x + p3d2.y*p3d2.y + p3d2.z*p3d2.z);
00137         float shadow_point_dist = float(pl / std::abs(tan(triangulate_angle*3.1415926535/180.))); // How far we need the shadow point to have the parallax limit match
00138 
00139         // Create the shadow point
00140         p3d_sh.x /= l;
00141         p3d_sh.y /= l;
00142         p3d_sh.z /= l;
00143         p3d_sh.x *= shadow_point_dist;
00144         p3d_sh.y *= shadow_point_dist;
00145         p3d_sh.z *= shadow_point_dist;
00146         p3d_sh.x += p3d.x;
00147         p3d_sh.y += p3d.y;
00148         p3d_sh.z += p3d.z;
00149 
00150         //std::cout<<"p  : "<<p3d.x<<","<<p3d.y<<","<<p3d.z<<std::endl; 
00151         //std::cout<<"psh: "<<p3d_sh.x<<","<<p3d_sh.y<<","<<p3d_sh.z<<std::endl;        
00152 }
00153 
00154 bool SimpleSfM::Update(IplImage *image, bool assume_plane, bool triangulate, float reproj_err_limit, float triangulate_angle) {
00155         const int min_triangulate=50, max_triangulate=150;
00156         const float plane_dist_limit = 100.f;
00157         const bool remember_3d_points = true;
00158         const float parallax_length = 10.f;
00159         const float parallax_length_sq = parallax_length*parallax_length;
00160         float reproj_err_limit_sq = reproj_err_limit*reproj_err_limit;
00161         std::map<int, Feature>::iterator  iter;
00162         std::map<int, Feature>::iterator iter_end = container.end();
00163 
00164         // #1. Detect marker corners and track features
00165         marker_detector.Detect(image, &cam, container, 0, 0, 1023, true, false);
00166         tf.Purge();
00167         tf.Track(image, 0, container, 1); // Track without adding features
00168         //tf.Track(image, 0, container, 1, 1024, 65535); // Track with adding features
00169         tf.EraseNonTracked(container, 1);
00170 
00171         // #2. Update pose
00172         if (!pose_ok) pose_ok = cam.CalcExteriorOrientation(container, &pose);
00173         pose_ok = cam.UpdatePose(container, &pose);
00174         if (!pose_ok) pose_ok = cam.UpdatePose(container, &pose, 0); // Only markers
00175         if (!pose_ok) return false;
00176 
00177         // #3. Reproject features and their shadows
00178         double err_markers  = cam.Reproject(container, &pose, 0, true, false, 100);
00179         if (err_markers > reproj_err_limit*2) { Reset(false); return false;}
00180         double err_features = cam.Reproject(container, &pose, 1, false, false, 100);
00181         if ((err_markers == 0) && (err_features > reproj_err_limit*2)) {
00182                 // Error is so large, that we just are satisfied we have some pose
00183                 // Don't triangulate and remove points based on this result
00184                 return true;
00185         }
00186 
00187         // #4. Add previously triangulated features to container if they are visible...
00188         if (remember_3d_points) {
00189                 IplImage *mask = tf.NewFeatureMask();
00190                 iter = container_triangulated.begin();
00191                 while(iter != container_triangulated.end()) {
00192                         if (container.find(iter->first) == container.end()) {
00193                                 Camera *c = &cam;
00194                                 c->ProjectPoint(iter->second.p3d, &pose, iter->second.projected_p2d);
00195                                 if ((iter->second.projected_p2d.x >= 0) &&
00196                                         (iter->second.projected_p2d.y >= 0) &&
00197                                         (iter->second.projected_p2d.x < image->width) &&
00198                                         (iter->second.projected_p2d.y < image->height))
00199                                 {
00200                                         CvScalar s = cvGet2D(mask, int(iter->second.projected_p2d.y), int(iter->second.projected_p2d.x));
00201                                         if (s.val[0] == 0) {
00202                                                 container[iter->first] = iter->second;
00203                                                 container[iter->first].estimation_type = 3;
00204                                                 container[iter->first].p2d = container[iter->first].projected_p2d;
00205                                         }
00206                                 }
00207                         }
00208                         iter++;
00209                 }
00210         }
00211 
00212         // #5. Add other features to container
00213         // Assume them initially on marker plane if (assume_plane == true)
00214         tf.AddFeatures(container, 1, 1024, 0xffffff); //65535);
00215         if (assume_plane) {
00216                 for (iter = container.begin(); iter != iter_end; iter++) {
00217                         Feature &f = iter->second;
00218                         //if (f.type_id == 0) continue;
00219                         if (!f.has_p3d && f.estimation_type < 1) {
00220                                 //cam.Get3dOnPlane(&pose, f.p2d, f.p3d);
00221 
00222                                 //CvPoint3D32f p3d_vec;
00223                                 //float l = PointVectorFromCamera(f.p3d, p3d_vec, &pose);
00224                                 //if (l > plane_dist_limit) cam.Get3dOnDepth(&pose, f.p2d, plane_dist_limit, f.p3d);
00225                                 cam.Get3dOnDepth(&pose, f.p2d, plane_dist_limit, f.p3d);
00226 
00227                                 // TODO: Now we don't create a shadow point for plane points. This is because
00228                                 //       we don't want to remember them as 3d points in container_triangulated.
00229                                 //       We probably get the same benefit just by assuming that everything is on plane?
00230                                 //CreateShadowPoint(f.p3d_sh, f.p3d, &pose, shadow_point_dist);
00231                                 f.p3d_sh = f.p3d;
00232 
00233                                 f.has_p3d = true;
00234                                 f.estimation_type = 1;
00235                         }
00236                 }
00237         }
00238 
00239         // #6. Triangulate features with good parallax
00240         if (triangulate) {
00241                 for (iter = container.begin(); iter != iter_end; iter++) {
00242                         Feature &f = iter->second;
00243                         //if (f.type_id == 0) continue;
00244                         if (!f.has_p3d && !f.has_stored_pose) {
00245                                 f.pose1 = pose;
00246                                 f.p2d1 = f.p2d;
00247                                 f.has_stored_pose = true;
00248                                 cam.Get3dOnDepth(&pose, f.p2d, 8, f.p3d);
00249                                 CreateShadowPoint(f.p3d_sh, f.p3d, &cam, &pose, parallax_length, triangulate_angle);
00250                         }
00251                         if (!f.has_p3d && f.has_stored_pose && f.estimation_type < 2) {
00252                                 double dist_sh_sq = (f.projected_p2d_sh.x-f.projected_p2d.x)*(f.projected_p2d_sh.x-f.projected_p2d.x);
00253                                 dist_sh_sq       += (f.projected_p2d_sh.y-f.projected_p2d.y)*(f.projected_p2d_sh.y-f.projected_p2d.y);
00254                                 if (dist_sh_sq > parallax_length_sq) {
00255                                         CvPoint2D32f u1 = f.p2d1;
00256                                         CvPoint2D32f u2 = f.p2d;
00257                                         cam.Camera::Undistort(u1);
00258                                         cam.Camera::Undistort(u2);
00259                                         if(cam.ReconstructFeature(&f.pose1, &pose, &u1, &u2, &f.p3d, 10e-6)) {
00260                                                 CreateShadowPoint(f.p3d_sh, f.p3d,  &cam, &pose, parallax_length, triangulate_angle);
00261                                                 f.has_p3d = true;
00262                                         } else
00263                                                 f.has_stored_pose = false;
00264                                         f.estimation_type = 2;
00265                                 }
00266                         }
00267                 }
00268         }
00269 
00270         // #7. Erase poor features
00271         cam.Reproject(container, &pose, 1, false, false, 100); // TODO: Why again...
00272         iter = container.begin();
00273         while(iter != container.end()) {
00274                 bool iter_inc = true;
00275                 if (iter->second.type_id > 0) {
00276                         Feature &f = iter->second;
00277                         if ((f.estimation_type == 0) || !f.has_p2d || !f.has_p3d);
00278                         else {
00279                                 // Note that the projected_p2d needs to have valid content at this point
00280                                 double dist_sq = (f.p2d.x-f.projected_p2d.x)*(f.p2d.x-f.projected_p2d.x);
00281                                 dist_sq       += (f.p2d.y-f.projected_p2d.y)*(f.p2d.y-f.projected_p2d.y);
00282 
00283                                 if (f.estimation_type == 1) {
00284                                         if (dist_sq > (reproj_err_limit_sq)) f.has_p3d = false;
00285                                 } else if (f.estimation_type == 2) {
00286                                         if (dist_sq > (reproj_err_limit_sq)) {
00287                                                 container.erase(iter++);
00288                                                 iter_inc = false;
00289                                         }
00290                                 } else if (f.estimation_type == 3) {
00291                                         if (container_triangulated.size() < min_triangulate) {
00292                                                 if (dist_sq > (reproj_err_limit_sq)) {
00293                                                         container.erase(iter++);
00294                                                         iter_inc = false;
00295                                                 }
00296                                         } else {
00297                                                 if (dist_sq > (reproj_err_limit_sq)) f.has_p3d = false;
00298                                                 if (dist_sq > (reproj_err_limit_sq*2)) {
00299                                                         std::map<int, Feature>::iterator iter_tmp;
00300                                                         iter_tmp = container_triangulated.find(iter->first);
00301                                                         if (iter_tmp != container_triangulated.end()) {
00302                                                                 container_triangulated.erase(iter_tmp);
00303                                                         }
00304                                                         container.erase(iter++);
00305                                                         iter_inc = false;
00306                                                 }
00307                                         }
00308                                 }
00309                         }
00310                 }
00311                 if (iter_inc) iter++;
00312         }
00313 
00314         // #8. Remember good features that have little reprojection error while the parallax is noticeable
00315         if (remember_3d_points && (container_triangulated.size() < max_triangulate)) {
00316                 iter = container.begin();
00317                 while(iter != container.end()) {
00318                         if ((iter->second.type_id > 0) &&
00319                                 (container_triangulated.find(iter->first) == container_triangulated.end()))
00320                         {
00321                                 Feature &f = iter->second;
00322                                 double dist_sq = (f.p2d.x-f.projected_p2d.x)*(f.p2d.x-f.projected_p2d.x);
00323                                 dist_sq       += (f.p2d.y-f.projected_p2d.y)*(f.p2d.y-f.projected_p2d.y);
00324                                 double dist_sh_sq = (f.projected_p2d_sh.x-f.projected_p2d.x)*(f.projected_p2d_sh.x-f.projected_p2d.x);
00325                                 dist_sh_sq       += (f.projected_p2d_sh.y-f.projected_p2d.y)*(f.projected_p2d_sh.y-f.projected_p2d.y);
00326                                 if ((dist_sq < reproj_err_limit_sq) && (dist_sh_sq > parallax_length_sq)) { 
00327                                         container_triangulated[iter->first] = iter->second;
00328                                         f.estimation_type = 3;
00329                                 }
00330                         }
00331                         iter++;
00332                         if (container_triangulated.size() >= max_triangulate) break;
00333                 }
00334         }
00335 
00336         return true;
00337 }
00338 
00339 bool SimpleSfM::UpdateTriangulateOnly(IplImage *image) {
00340         marker_detector.Detect(image, &cam, container, 0, 0, 1023, true, false);
00341         tf.Purge();
00342         tf.Track(image, 0, container, 1, 1024, 65535);
00343         tf.EraseNonTracked(container, 1);
00344         if (!pose_ok) pose_ok = cam.CalcExteriorOrientation(container, &pose);
00345         else pose_ok = cam.UpdatePose(container, &pose);
00346         if(pose_ok) update_tri = moving.UpdateDistance(&pose, scale);
00347         std::map<int,Feature>::iterator iter = container.begin();
00348         std::map<int,Feature>::iterator iter_end = container.end();
00349         for(;iter != iter_end; iter++) {
00350                 if (pose_ok && (iter->second.type_id == 1) && (!iter->second.has_stored_pose) && (!iter->second.has_p3d)) {
00351                         iter->second.pose1 = pose;
00352                         iter->second.p2d1 = iter->second.p2d;
00353                         iter->second.has_stored_pose = true;
00354                 }
00355         }
00356         iter = container.begin();
00357         for(;iter != iter_end; iter++)
00358         {
00359                 Feature &f = iter->second;
00360                 if(update_tri && f.has_stored_pose && !f.has_p3d) {
00361                         f.tri_cntr++;
00362                 }
00363                 if(f.tri_cntr==3) {
00364                         CvPoint2D32f u1 = f.p2d1;
00365                         CvPoint2D32f u2 = f.p2d;
00366                         cam.Camera::Undistort(u1);
00367                         cam.Camera::Undistort(u2);
00368                         if(cam.ReconstructFeature(&f.pose1, &pose, &u1, &u2, &f.p3d, 10e-6))
00369                                 f.has_p3d = true;
00370                         else
00371                                 f.has_stored_pose = false;
00372                         f.tri_cntr = 0;
00373                 }
00374         }
00375         if (pose_ok) {
00376                 double err_markers  = cam.Reproject(container, &pose, 0, true, false, 100);
00377                 if(err_markers > 30) { Reset(false); return false;}
00378                 double err_features = cam.Reproject(container, &pose, 1, false, false, 100);
00379                 cam.EraseUsingReprojectionError(container, 30, 1);
00380         }
00381         return pose_ok;
00382 }
00383 
00384 bool SimpleSfM::UpdateRotationsOnly(IplImage *image) {
00385         int n_markers = marker_detector.Detect(image, &cam, container, 0, 0, 1023, true, false);
00386         static int n_markers_prev = 0;
00387         tf.Purge();
00388         tf.Track(image, 0, container, 1, 1024, 65535);
00389         tf.EraseNonTracked(container, 1);
00390         int type_id = -1;
00391         if(n_markers>=1) type_id = 0;
00392         if (n_markers==1)
00393         {
00394                 pose_ok = cam.CalcExteriorOrientation(container, &pose, 0);
00395                 if(pose_ok) pose_original = pose;
00396         }
00397         else if(n_markers>1)
00398         {
00399                 if(pose_ok)
00400                         pose_ok = cam.UpdatePose(container, &pose, 0);
00401                 else
00402                         pose_ok = cam.CalcExteriorOrientation(container, &pose, 0);
00403                 if(pose_ok) pose_original = pose;
00404         }
00405         else //if(pose_ok) // Tracking going on...
00406         {
00407                 if (n_markers_prev > 0) {
00408                         pose_difference.Reset();
00409                         pose_difference.Mirror(false, true, true);
00410                         std::map<int,Feature>::iterator iter = container.begin();
00411                         std::map<int,Feature>::iterator iter_end = container.end();
00412                         for(;iter != iter_end; iter++) {
00413                                 if ((iter->second.type_id == 1) && (iter->second.has_p2d)) {
00414                                         CvPoint2D32f _p2d = iter->second.p2d;
00415                                         cam.Get3dOnDepth(&pose_difference, _p2d, 1, iter->second.p3d);
00416                                         iter->second.has_p3d = true;
00417                                 }
00418                         }
00419                 }
00420                 cam.UpdateRotation(container, &pose_difference, 1);
00421                 if(pose_ok)
00422                 {
00423                         double rot_mat[16];
00424                         double gl_mat[16];
00425                         pose_difference.GetMatrixGL(rot_mat);
00426                         pose_original.GetMatrixGL(gl_mat);
00427                         CvMat rot = cvMat(4, 4, CV_64F, rot_mat);// Rotation matrix (difference) from the tracker
00428                         CvMat mod = cvMat(4, 4, CV_64F, gl_mat); // Original modelview matrix (camera location)
00429                         cvMatMul(&mod, &rot, &rot);
00430                         /*if(pose_ok)*/ pose.SetMatrixGL(rot_mat);
00431                 }
00432                 //pose_ok = true;
00433         }
00434         n_markers_prev = n_markers;
00435         if (pose_ok) {
00436                 if (n_markers < 1) {
00437                         std::map<int,Feature>::iterator iter = container.begin();
00438                         std::map<int,Feature>::iterator iter_end = container.end();
00439                         for(;iter != iter_end; iter++) {
00440                                 if ((iter->second.type_id == 1) && (!iter->second.has_p3d) && (iter->second.has_p2d)) {
00441                                         CvPoint2D32f _p2d = iter->second.p2d;
00442                                         cam.Get3dOnDepth(&pose_difference, _p2d, 1, iter->second.p3d);
00443                                         iter->second.has_p3d = true;
00444                                 }
00445                         }
00446                 }
00447 
00448                 double err_markers  = cam.Reproject(container, &pose, 0, true, false, 100);
00449                 if(err_markers > 10) { Reset(false); return false;}
00450                 double err_features = cam.Reproject(container, &pose_difference, 1, false, false, 100);
00451                 cam.EraseUsingReprojectionError(container, 10, 1);
00452         }
00453         return pose_ok;
00454 }
00455 
00456 void SimpleSfM::Draw(IplImage *rgba) {
00457         if(markers_found) return;
00458         std::map<int,Feature>::iterator iter = container.begin();
00459         std::map<int,Feature>::iterator iter_end = container.end();
00460         for(;iter != iter_end;iter++) {
00461                 Feature &f = iter->second;
00462                 if (f.has_p2d) {
00463                         int r=0, g=0, b=0, rad=1;
00464                         if (f.type_id == 0)              {r=255; g=0; b=0;}
00465                         else if (f.estimation_type == 0) {
00466                                 if (f.has_stored_pose) {r=255; g=0; b=255;}
00467                                 else                   {r=0; g=255; b=255;}
00468                         }
00469                         else if (f.estimation_type == 1) {r=0; g=255; b=0;}
00470                         else if (f.estimation_type == 2) {r=255; g=0; b=255;}
00471                         else if (f.estimation_type == 3) {r=0; g=0; b=255;}
00472                         if (f.has_p3d) rad=5;
00473                         else rad = f.tri_cntr+1;
00474 
00475                         cvCircle(rgba, cvPointFrom32f(f.p2d), rad, CV_RGB(r,g,b));
00476                         if (pose_ok) {
00477                                 // The shadow point line
00478                                 if (f.type_id > 0 && f.estimation_type < 3 && f.p3d_sh.x != 0.f) {
00479                                         cvLine(rgba, cvPointFrom32f(f.projected_p2d), cvPointFrom32f(f.projected_p2d_sh), CV_RGB(0,255,0));
00480                                 }
00481                                 // Reprojection error
00482                                 if (f.has_p3d) {
00483                                         cvLine(rgba, cvPointFrom32f(f.p2d), cvPointFrom32f(f.projected_p2d), CV_RGB(255,0,255));
00484                                 }
00485                         }
00486                         //if (pose_ok && f.has_p3d) {
00487                         //      cvLine(rgba, cvPointFrom32f(f.p2d), cvPointFrom32f(f.projected_p2d), CV_RGB(255,0,255));
00488                         //}
00489                         //if (f.type_id == 0) {
00490                         //      int id = iter->first;
00491                         //      cvCircle(rgba, cvPointFrom32f(f.p2d), 7, CV_RGB(255,0,0));
00492                         //} else {
00493                         //      int id = iter->first-1024+1;
00494                         //      if (f.has_p3d) { 
00495                         //              cvCircle(rgba, cvPointFrom32f(f.p2d), 5, CV_RGB(0,255,0));
00496                         //      }
00497                         //      else if (f.has_stored_pose) 
00498                         //              cvCircle(rgba, cvPointFrom32f(f.p2d), f.tri_cntr+1, CV_RGB(255,0,255));
00499                         //      else 
00500                         //              cvCircle(rgba, cvPointFrom32f(f.p2d), 5, CV_RGB(0,255,255));
00501                         //}
00502                 }
00503         }
00504 }
00505 
00506 } // namespace alvar
00507 


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