Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

visionperceptor.cpp

Go to the documentation of this file.
00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
00002 
00003    this file is part of rcssserver3D
00004    Fri May 9 2003
00005    Copyright (C) 2002,2003 Koblenz University
00006    Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
00007    $Id: visionperceptor.cpp,v 1.16 2006/03/12 14:11:22 fruit Exp $
00008 
00009    This program is free software; you can redistribute it and/or modify
00010    it under the terms of the GNU General Public License as published by
00011    the Free Software Foundation; version 2 of the License.
00012 
00013    This program is distributed in the hope that it will be useful,
00014    but WITHOUT ANY WARRANTY; without even the implied warranty of
00015    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016    GNU General Public License for more details.
00017 
00018    You should have received a copy of the GNU General Public License
00019    along with this program; if not, write to the Free Software
00020    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00021 */
00022 #include "visionperceptor.h"
00023 #include <zeitgeist/logserver/logserver.h>
00024 #include <oxygen/sceneserver/scene.h>
00025 #include <oxygen/sceneserver/transform.h>
00026 #include <soccer/soccerbase/soccerbase.h>
00027 
00028 using namespace zeitgeist;
00029 using namespace oxygen;
00030 using namespace boost;
00031 using namespace salt;
00032 
00033 VisionPerceptor::VisionPerceptor() : Perceptor(),
00034                                      mSenseMyPos(false),
00035                                      mAddNoise(true),
00036                                      mUseRandomNoise(true),
00037                                      mStaticSenseAxis(true)
00038 {
00039     // set predicate name
00040     SetPredicateName("Vision");
00041     // set some default noise values
00042     SetNoiseParams(0.0965, 0.1225, 0.1480, 0.005);
00043 }
00044 
00045 VisionPerceptor::~VisionPerceptor()
00046 {
00047     mDistRng.reset();
00048     mPhiRng.reset();
00049     mThetaRng.reset();
00050 }
00051 
00052 void
00053 VisionPerceptor::SetNoiseParams(float sigma_dist, float sigma_phi,
00054                                 float sigma_theta, float cal_error_abs)
00055 {
00056     mSigmaDist = sigma_dist;
00057     mSigmaPhi = sigma_phi;
00058     mSigmaTheta = sigma_theta;
00059     mCalErrorAbs = cal_error_abs;
00060 
00061     NormalRngPtr rng1(new salt::NormalRNG<>(0.0,sigma_dist));
00062     mDistRng = rng1;
00063     NormalRngPtr rng2(new salt::NormalRNG<>(0.0,sigma_phi));
00064     mPhiRng = rng2;
00065     NormalRngPtr rng3(new salt::NormalRNG<>(0.0,sigma_theta));
00066     mThetaRng = rng3;
00067 
00068     salt::UniformRNG<float> rng4(-mCalErrorAbs,mCalErrorAbs);
00069     mError = salt::Vector3f(rng4(),rng4(),rng4());
00070 }
00071 
00072 void
00073 VisionPerceptor::OnLink()
00074 {
00075     SoccerBase::GetTransformParent(*this,mTransformParent);
00076     SoccerBase::GetAgentState(*this, mAgentState);
00077     SoccerBase::GetActiveScene(*this,mActiveScene);
00078 }
00079 
00080 void
00081 VisionPerceptor::OnUnlink()
00082 {
00083     mDistRng.reset();
00084     mPhiRng.reset();
00085     mThetaRng.reset();
00086     mTransformParent.reset();
00087     mAgentState.reset();
00088     mActiveScene.reset();
00089 }
00090 
00091 void
00092 VisionPerceptor::AddNoise(bool add_noise)
00093 {
00094     mAddNoise = add_noise;
00095 }
00096 
00097 void
00098 VisionPerceptor::UseRandomNoise(bool random_noise)
00099 {
00100     mUseRandomNoise = random_noise;
00101 }
00102 
00103 void
00104 VisionPerceptor::SetStaticSenseAxis(bool static_axis)
00105 {
00106     mStaticSenseAxis = static_axis;
00107 }
00108 
00109 bool
00110 VisionPerceptor::ConstructInternal()
00111 {
00112     mRay = shared_static_cast<oxygen::RayCollider>
00113         (GetCore()->New("oxygen/RayCollider"));
00114 
00115     if (mRay.get() == 0)
00116     {
00117         GetLog()->Error() << "Error: (VisionPerceptor) cannot create Raycollider. "
00118                           << "occlusion check disabled\n";
00119     }
00120 
00121     return true;
00122 }
00123 
00124 void
00125 VisionPerceptor::SetupVisibleObjects(TObjectList& visibleObjects)
00126 {
00127     TLeafList objectList;
00128     mActiveScene->ListChildrenSupportingClass<ObjectState>(objectList, true);
00129 
00130     salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00131 
00132     for (TLeafList::iterator i = objectList.begin();
00133          i != objectList.end(); ++i)
00134         {
00135             ObjectData od;
00136             od.mObj = shared_static_cast<ObjectState>(*i);
00137 
00138             if (od.mObj.get() == 0)
00139                 {
00140                     GetLog()->Error() << "Error: (VisionPerceptor) skipped: "
00141                                       << (*i)->GetName() << "\n";
00142                     continue; // this should never happen
00143                 }
00144 
00145             shared_ptr<Transform> j = od.mObj->GetTransformParent();
00146 
00147             if (j.get() == 0)
00148                 {
00149                     continue; // this should never happen
00150                 }
00151 
00152             od.mRelPos = j->GetWorldTransform().Pos() - myPos;
00153             od.mDist   = od.mRelPos.Length();
00154 
00155             visibleObjects.push_back(od);
00156         }
00157 }
00158 
00159 void VisionPerceptor::AddSense(oxygen::Predicate& predicate, ObjectData& od) const
00160 {
00161     ParameterList& element = predicate.parameter.AddList();
00162     element.AddValue(od.mObj->GetPerceptName());
00163 
00164     if(od.mObj->GetPerceptName() == "Player")
00165         {
00166             ParameterList player;
00167             player.AddValue(std::string("team"));
00168             player.AddValue
00169                 (std::string
00170                  (od.mObj->GetPerceptName(ObjectState::PT_Player)
00171                   )
00172                  );
00173             element.AddValue(player);
00174         }
00175 
00176     if (!od.mObj->GetID().empty())
00177         {
00178             ParameterList id;
00179             id.AddValue(std::string("id"));
00180             id.AddValue(od.mObj->GetID());
00181             element.AddValue(id);
00182         }
00183 
00184     ParameterList& position = element.AddList();
00185     position.AddValue(std::string("pol"));
00186     position.AddValue(od.mDist);
00187     position.AddValue(od.mTheta);
00188     position.AddValue(od.mPhi);
00189 }
00190 
00191 void
00192 VisionPerceptor::ApplyNoise(ObjectData& od) const
00193 {
00194     if (mAddNoise)
00195         {
00196             if (mUseRandomNoise)
00197                 {
00198                     od.mDist  += (*(mDistRng.get()))() * od.mDist / 100.0;
00199                     od.mTheta += (*(mThetaRng.get()))();
00200                     od.mPhi   += (*(mPhiRng.get()))();
00201                 } else
00202                 {
00203                     /* This gives a constant random error throughout the whole
00204                      * match. This behavior was not intended and is a bug and
00205                      * not an intended feature.
00206                      * It was kept in the simulator because I discovered this
00207                      * bug only shortly before the competition. *sigh* oliver
00208                      */
00209                     od.mDist  += salt::NormalRNG<>(0.0,mSigmaDist)();
00210                     od.mTheta += salt::NormalRNG<>(0.0,mSigmaTheta)();
00211                     od.mPhi  += salt::NormalRNG<>(0.0,mSigmaPhi)();
00212                 }
00213         }
00214 }
00215 
00216 
00217 bool
00218 VisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
00219 {
00220     Predicate& predicate = predList->AddPredicate();
00221     predicate.name       = mPredicateName;
00222     predicate.parameter.Clear();
00223 
00224     TTeamIndex  ti       = mAgentState->GetTeamIndex();
00225     salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00226 
00227     TObjectList visibleObjects;
00228     SetupVisibleObjects(visibleObjects);
00229 
00230     for (std::list<ObjectData>::iterator i = visibleObjects.begin();
00231          i != visibleObjects.end(); ++i)
00232     {
00233         ObjectData& od = (*i);
00234 
00235         od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
00236         if (mAddNoise)
00237             {
00238                 od.mRelPos += mError;
00239             }
00240 
00241         if (
00242             (od.mRelPos.Length() <= 0.1) ||
00243             (CheckOcclusion(myPos,od))
00244             )
00245         {
00246             // object is occluded or too close
00247             continue;
00248         }
00249 
00250         // theta is the angle in the X-Y (horizontal) plane
00251         od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0]));
00252 
00253         // latitude
00254         od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist));
00255 
00256         // make some noise
00257         ApplyNoise(od);
00258 
00259         // generate a sense entry
00260         AddSense(predicate,od);
00261     }
00262 
00263     if (mSenseMyPos)
00264     {
00265         Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
00266 
00267         ParameterList& element = predicate.parameter.AddList();
00268         element.AddValue(std::string("mypos"));
00269         element.AddValue(sensedMyPos[0]);
00270         element.AddValue(sensedMyPos[1]);
00271         element.AddValue(sensedMyPos[2]);
00272     }
00273 
00274     return true;
00275 }
00276 
00277 bool
00278 VisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
00279 {
00280     Predicate& predicate = predList->AddPredicate();
00281     predicate.name       = mPredicateName;
00282     predicate.parameter.Clear();
00283 
00284     TTeamIndex  ti          = mAgentState->GetTeamIndex();
00285 
00286     const Vector3f& up = mTransformParent->GetWorldTransform().Up();
00287 
00288     // calc the percptors angle in the horizontal plane
00289     double fwTheta = gNormalizeRad(Vector2f(up[0],up[1]).GetAngleRad());
00290 
00291     // calc the perceptors angle in the vertical plane
00292     double fwPhi = gNormalizeRad(Vector2f(up[0],up[2]).GetAngleRad());
00293 
00294     TObjectList visibleObjects;
00295     SetupVisibleObjects(visibleObjects);
00296 
00297     for (std::list<ObjectData>::iterator i = visibleObjects.begin();
00298          i != visibleObjects.end(); ++i)
00299     {
00300         ObjectData& od = (*i);
00301 
00302         od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
00303         if (mAddNoise)
00304             {
00305                 od.mRelPos += mError;
00306             }
00307 
00308         if (od.mRelPos.Length() <= 0.1)
00309         {
00310             // object is too close
00311             continue;
00312         }
00313 
00314         // theta is the angle in horizontal plane, with fwAngle as 0 degree
00315         od.mTheta = gRadToDeg(gNormalizeRad(
00316                               Vector2f(od.mRelPos[0],od.mRelPos[1]).GetAngleRad() -
00317                               fwTheta
00318                               ));
00319 
00320         // latitude with fwPhi as 0 degreee
00321         od.mPhi = gRadToDeg(gNormalizeRad(
00322                             Vector2f(od.mRelPos[0],od.mRelPos[2]).GetAngleRad() -
00323                             fwPhi
00324                             ));
00325 
00326         // make some noise
00327         ApplyNoise(od);
00328 
00329         // generate a sense entry
00330         AddSense(predicate,od);
00331     }
00332 
00333     return true;
00334 }
00335 
00336 bool
00337 VisionPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
00338 {
00339     if (
00340         (mTransformParent.get() == 0) ||
00341         (mActiveScene.get() == 0) ||
00342         (mAgentState.get() == 0)
00343         )
00344     {
00345         return false;
00346     }
00347 
00348     return mStaticSenseAxis ?
00349         StaticAxisPercept(predList) :
00350         DynamicAxisPercept(predList);
00351 }
00352 
00353 bool VisionPerceptor::CheckOcclusion(const Vector3f& my_pos, const ObjectData& od) const
00354 {
00355     // (occlusion test disabled for now, every object is visible)
00356     return false;
00357 
00358 //     if (mRay.get() == 0) return;
00359 
00360 //     // sort objects wrt their distance
00361 //     visible_objects.sort();
00362 //     // check visibility
00363 //     std::list<ObjectData>::iterator start = visible_objects.begin();
00364 //     ++start;
00365 
00366 //     // this is going to be expensive now: to check occlusion of an object,
00367 //     // we have to check all closer objects. For n objects, we have to
00368 //     // check at most n*(n-1)/2 objects.
00369 //     for (std::list<ObjectData>::iterator i = start;
00370 //          i != visible_objects.end(); ++i)
00371 //     {
00372 //         for (std::list<ObjectData>::iterator j = visible_objects.begin();
00373 //              j != i; ++j)
00374 //         {
00375 //             // cast ray from camera to object (j)
00376 //             mRay->SetParams(j->mRelPos,my_pos,j->mDist);
00377 
00378 //             dContactGeom contact;
00379 
00380 //             shared_ptr<Collider> collider = shared_static_cast<Collider>
00381 //                 (i->mObj->GetChildSupportingClass("Collider"));
00382 
00383 //             if (mRay->Intersects(collider))
00384 //                 {
00385 //                     j->mVisible = false;
00386 //                     j = i;
00387 //                 }
00388 //         }
00389 //     }
00390 }
00391 
00392 void
00393 VisionPerceptor::SetSenseMyPos(bool sense)
00394 {
00395     mSenseMyPos = sense;
00396 }

Generated on Thu Apr 6 15:25:40 2006 for rcssserver3d by  doxygen 1.4.4