Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Point.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include <math.h>
00004 
00005 #include "Shared/Config.h"
00006 #include "Motion/Kinematics.h"
00007 
00008 #include "Shared/Measures.h"
00009 #include "Point.h"
00010 #include "LineData.h"
00011 #include "ShapeTypes.h"  // for ReferenceFrameType_t
00012 #include "VRmixin.h"
00013 
00014 using namespace std;
00015 
00016 namespace DualCoding {
00017 
00018   Point::Point(void) : coords(4), refFrameType(unspecified) {
00019   coords << 0 << 0 << 0 << 1;
00020   }
00021 
00022 Point::Point(coordinate_t const &xp, coordinate_t const &yp, coordinate_t zp, 
00023        ReferenceFrameType_t ref) : coords(4), refFrameType(ref) {
00024   coords << xp << yp << zp << 1;
00025 };
00026 
00027 Point::Point(const NEWMAT::ColumnVector& c, ReferenceFrameType_t ref) : 
00028   coords(4), refFrameType(ref) { coords = c; }
00029 
00030 //!Set Position
00031 //{
00032 void Point::setCoords(const Point& otherPt) {
00033   coords = otherPt.coords;
00034 }
00035 
00036 void Point::setCoords(coordinate_t x, coordinate_t y, coordinate_t z) {
00037   coords << x << y << z << 1;
00038 }
00039 //}
00040 
00041 float Point::distanceFrom(const Point& otherPt) const {
00042   return (coords - otherPt.coords).NormFrobenius();
00043 }
00044 
00045 float Point::xyDistanceFrom(const Point& other) const {
00046   float dx = coords(1)-other.coords(1);
00047   float dy = coords(2)-other.coords(2);
00048   return sqrt(dx*dx+dy*dy);
00049 }
00050 
00051 float Point::xyNorm() const { return sqrt(coords(1)*coords(1) + coords(2)*coords(2)); }
00052 
00053 bool Point::isLeftOf(const Point& other, float distance) const {
00054   switch ( refFrameType ) {
00055   case camcentric:
00056     return coordX()+distance < other.coordX();
00057   case egocentric:  
00058     return coordY()-distance > other.coordY();
00059   case allocentric:
00060   default:
00061     cout << "Allocentric Point::isLeftOf fudged for now" << endl;
00062     return coordY()-distance > other.coordY();
00063     // Should really calculate bearings to both points from current
00064     // agent location and check sign of the difference
00065   }
00066 }
00067 
00068 bool Point::isRightOf(const Point& other, float distance) const {
00069   return other.isLeftOf(*this,distance); }
00070 
00071 bool Point::isAbove(const Point& other,float distance) const {
00072   switch ( refFrameType ) {
00073   case camcentric:
00074     return coordY()+distance < other.coordY();
00075   case egocentric:  
00076     return coordX()-distance > other.coordX();
00077   case allocentric:
00078   default:
00079     cout << "Allocentric Point::isLeftOf fudged for now" << endl;
00080     return coordX()-distance > other.coordX();
00081     // Should really calculate bearings to both points from current
00082     // agent location and check sign of the difference
00083   }
00084 }
00085 
00086 bool Point::isBelow(const Point& other, float distance) const {
00087   return other.isAbove(*this,distance); }
00088 
00089 
00090 //! Apply a transformation matrix to translate and/or rotate  the point.
00091 void Point::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00092   coords = Tmat*coords;
00093   if ( newref != unspecified )
00094     refFrameType = newref;
00095  }
00096 
00097 void Point::projectToGround(const NEWMAT::Matrix& camToBase,
00098           const NEWMAT::ColumnVector& groundPlane) {
00099   NEWMAT::Matrix T2 = camToBase.i();
00100   T2.SubMatrix(4,4,1,3)=T2.SubMatrix(1,3,4,4).t()*T2.SubMatrix(1,3,1,3);
00101   T2.SubMatrix(1,3,4,4)=0;
00102   //cout << "Transform plane b->j:\n"<<T2;
00103 
00104   const float normX = 2*(coordX()/VRmixin::camSkS.getWidth()) - 1;
00105   const float normY = 2*(coordY()/VRmixin::camSkS.getHeight()) - 1;
00106   NEWMAT::ColumnVector camera_point(4), camPlane(4);
00107   config->vision.computeRay(normX, normY,
00108           camera_point(1),camera_point(2),camera_point(3));
00109   camera_point(4) = 1;
00110   camPlane = T2*groundPlane;
00111   
00112   float denom=0;
00113   for(unsigned int i=1; i<=3; i++)
00114     denom+=camera_point(i)*camPlane(i);
00115   NEWMAT::ColumnVector intersect=camera_point;
00116   if(denom==0)
00117     intersect(4)=0;
00118   else {
00119     float s=camPlane(4)/denom;
00120     for(unsigned int i=1; i<=3; i++)
00121       intersect(i)*=s;
00122     intersect(4)=1;
00123   }
00124   coords = camToBase*intersect;
00125   refFrameType = egocentric;
00126 }
00127 
00128 void Point::projectToGround(int xres, int yres,
00129           const NEWMAT::ColumnVector& ground_plane) {
00130 #ifdef TGT_HAS_CAMERA
00131   // Normalize coordinate system to [-1,+1]
00132   const float normX = 2*(coordX()/xres) - 1;
00133   const float normY = 2*(coordY()/yres) - 1;
00134   NEWMAT::ColumnVector camera_point(4), ground_point(4);
00135   config->vision.computeRay(normX, normY,
00136           camera_point(1),camera_point(2),camera_point(3));
00137   camera_point(4) = 1;
00138   ground_point = 
00139     kine->projectToPlane(CameraFrameOffset, camera_point,
00140        BaseFrameOffset, ground_plane, BaseFrameOffset);
00141   //  std::cout << "Ground plane: " << NEWMAT::printmat(ground_plane) << 
00142   //    "  camera_point: " << NEWMAT::printmat(camera_point) << 
00143   //    "  ground_point: " << NEWMAT::printmat(ground_point) << std::endl;
00144   coords(1) = ground_point(1) / ground_point(4);
00145   coords(2) = ground_point(2) / ground_point(4);
00146   coords(3) = ground_point(3) / ground_point(4);
00147 #endif
00148 }
00149   
00150 
00151 // Calling point and groundPoint must both have been projectToGround'ed already
00152 float Point::getHeightAbovePoint(const Point& groundPoint, const NEWMAT::ColumnVector& groundplane) {
00153 #ifndef TGT_HAS_CAMERA
00154   return groundPoint.coordZ();
00155 #else
00156   float camX, camY, camZ;
00157   NEWMAT::Matrix baseToCam = kine->jointToBase(CameraFrameOffset);
00158   NEWMAT::ColumnVector camOffset = baseToCam.Column(4);
00159   //std::cout<<std::endl;
00160   //std::cout<<"cam offset = "<<NEWMAT::printmat(camOffset)<<std::endl;
00161   //std::cout<<"groundPlane = "<<NEWMAT::printmat(groundplane)<<std::endl;
00162   camOffset = camOffset - groundplane;
00163   camOffset(4) = 1;
00164   
00165   Kinematics::unpack(camOffset, camX, camY, camZ);
00166   //std::cout<<"top pt coords = "<<NEWMAT::printmat(coords)<<std::endl;
00167   float dx = camX - coords(1);
00168   float dy = camY - coords(2);
00169   float dz = camZ - coords(3);
00170   //std::cout<<"Cam to point = "<<dx<<" "<<dy<<" "<<dz<<std::endl;
00171   float dHorizCam = sqrt(dx*dx + dy*dy);
00172 
00173   Point dP = groundPoint - *this;
00174   float dHorizPoint = sqrt(dP.coordX()*dP.coordX() + dP.coordY()*dP.coordY());
00175   
00176   return dz*dHorizPoint/dHorizCam;
00177 #endif
00178 }
00179 
00180 
00181 Point Point::operator+ (const Point& b) const { return Point(*this) += b; }
00182 
00183 Point& Point::operator+= (const Point& b) {
00184   coords += b.coords;
00185   makeRefFrameCompatible(b);
00186   return *this;
00187 }
00188 
00189 Point Point::operator- (const Point& b) const { return Point(*this) -= b; }
00190 
00191 Point& Point::operator-= (const Point& b) {
00192   coords -= b.coords;
00193   makeRefFrameCompatible(b);
00194   return *this;
00195 }
00196 
00197 void Point::makeRefFrameCompatible(const Point &other) {
00198   refFrameType = ( refFrameType == unspecified || refFrameType == other.refFrameType ) ?
00199     other.refFrameType : unspecified;
00200 }
00201 
00202 Point Point::operator* (float const b) const { return Point(coords*b,refFrameType); }
00203 
00204 Point& Point::operator *= (float const b) {
00205   coords *= b;
00206   return *this;
00207 }
00208 
00209 
00210 Point Point::operator/ (float const b) const { return Point(coords/b,refFrameType); }
00211 
00212 Point& Point::operator /= (float const b) {
00213   coords /= b;
00214   return *this;
00215 }
00216 
00217 bool Point::operator ==(const Point& b) const {
00218   return (coordX() == b.coordX() 
00219     && coordY() == b.coordY() 
00220     && coordZ() == b.coordZ());
00221 }
00222 
00223 std::ostream& operator<< (std::ostream& out, const Point &p) {
00224   switch ( p.refFrameType ) {
00225   case unspecified:
00226     out << "u:";
00227     break;
00228   case camcentric:
00229     out << "c:";
00230     break;
00231   case egocentric:
00232     out << "e:";
00233     break;
00234   case allocentric:
00235     out << "a:";
00236     break;
00237   }
00238   out << "[" << p.coordX() << ", " << p.coordY() 
00239       << ", " << p.coordZ() << "]";
00240   return out;
00241 }
00242 
00243 void Point::printData()
00244 {
00245   cout<<"{"<<coordX()<<","<<coordY()<<","<<coordZ()<<"}";
00246 }
00247 
00248 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4