objectprojection.cpp
Go to the documentation of this file.
00001 #include <openrave-core.h>
00002 #include <vector>
00003 #include <cstring>
00004 #include <sstream>
00005 
00006 using namespace OpenRAVE;
00007 using namespace std;
00008 
00013 void GetContactPointsFromImageRays(KinBodyPtr pbody, const vector<Vector>& vcamerapoints, const Transform& tcamera, vector<Vector>& vobjectpoints)
00014 {
00015     CollisionReportPtr report(new CollisionReport());
00016     TransformMatrix tmcamera(tcamera);
00017     KinBody::KinBodyStateSaver saver(pbody);
00018     pbody->SetTransform(Transform());
00019     AABB ab = pbody->ComputeAABB();
00020     dReal maxdist = RaveFabs(tmcamera.m[2]*tmcamera.trans.x + tmcamera.m[6]*tmcamera.trans.y + tmcamera.m[10]*tmcamera.trans.z)+RaveSqrt(ab.extents.lengthsqr3())+0.2f;
00021     RAY r;
00022     for(size_t i = 0; i < vcamerapoints.size(); ++i) {
00023         r.dir = tcamera.rotate(vcamerapoints[i]*(maxdist/RaveSqrt(vcamerapoints[i].lengthsqr3())));
00024         r.pos = tcamera.trans;
00025         if( pbody->GetEnv()->CheckCollision(r,pbody,report) ) {
00026             vobjectpoints.push_back(report->contacts.at(0).pos);
00027         }
00028         else {
00029             vobjectpoints.push_back(Vector(1.0/0,1.0/0,1.0/0)); // nan
00030         }
00031     }
00032 }
00033 
00034 int main(int argc, char ** argv)
00035 {
00036     EnvironmentBasePtr penv = CreateEnvironment(true);
00037     KinBodyPtr pbody = penv->ReadKinBodyXMLFile("brkt_hvac.kinbody.xml");
00038     penv->AddKinBody(pbody);
00039     vector<Vector> vcamerapoints, vobjectpoints;
00040     Transform tcamera;
00041     tcamera.trans.z = -1.8;
00042     vcamerapoints.push_back(Vector(0,0,1));
00043     GetContactPointsFromImageRays(pbody,vcamerapoints,tcamera,vobjectpoints);
00044     printf("%f %f %f\n",vobjectpoints[0].x,vobjectpoints[0].y,vobjectpoints[0].z);
00045     RaveDestroy();
00046     return 0;
00047 }


posedetectiondb
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Tue Jan 27 2015 11:53:11