00001
00002
00003 #include <iostream>
00004 #include <vector>
00005 #include <fstream>
00006
00007 #include "ShapeRoot.h"
00008 #include "ShapeLine.h"
00009 #include "ShapeEllipse.h"
00010 #include "ShapeBlob.h"
00011 #include "ShapePoint.h"
00012
00013 #include "ParticleShapes.h"
00014
00015 using namespace std;
00016
00017 namespace DualCoding {
00018
00019 void PfRoot::loadLms(const std::vector<ShapeRoot> &lms, bool isWorld, std::vector<PfRoot*>& landmarks){
00020 int id;
00021 int type;
00022 rgb color;
00023 bool mobile;
00024 deleteLms(landmarks);
00025 landmarks.reserve(lms.size());
00026 for (unsigned int i = 0; i<lms.size(); i++){
00027 type = lms[i]->getType();
00028 color = lms[i]->getColor();
00029 mobile = lms[i]->getMobile();
00030 if (type == lineDataType) {
00031 const Shape<LineData> &line = ShapeRootTypeConst(lms[i],LineData);
00032 id = line->getId();
00033 const EndPoint &pt1 = line->end1Pt();
00034 const EndPoint &pt2 = line->end2Pt();
00035 PfLine *land = new PfLine(id, color, mobile, pt1.coordX(), pt1.coordY(),
00036 pt2.coordX(), pt2.coordY(), pt1.isValid(), pt2.isValid());
00037 land->link = &lms[i];
00038 land->orientation = atan2(pt2.coordY()-pt1.coordY(), pt2.coordX()-pt1.coordX());
00039 land->length = sqrt((pt2.coordX()-pt1.coordX())*(pt2.coordX()-pt1.coordX()) +
00040 (pt2.coordY()-pt1.coordY())*(pt2.coordY()-pt1.coordY()));
00041 landmarks.push_back(land);
00042 if ( isWorld ) {
00043 PfLine *land2 = new PfLine(id, color, mobile, pt2.coordX(), pt2.coordY(),
00044 pt1.coordX(), pt1.coordY(), pt2.isValid(), pt1.isValid());
00045 land2->link = &lms[i];
00046 land2->orientation = land->orientation;
00047 land2->length = land->length;
00048 landmarks.push_back(land2);
00049 }
00050 }
00051 else if (type == ellipseDataType) {
00052 const Shape<EllipseData> &ellipse = ShapeRootTypeConst(lms[i],EllipseData);
00053 id = ellipse->getId();
00054 const Point &pt = ellipse->getCentroid();
00055 PfEllipse* land = new PfEllipse(id, color, mobile, pt.coordX(), pt.coordY());
00056 land->link = &lms[i];
00057 landmarks.push_back(land);
00058 }
00059 else if (type == pointDataType) {
00060 const Shape<PointData> &point = ShapeRootTypeConst(lms[i],PointData);
00061 id = point->getId();
00062 const Point &pt = point->getCentroid();
00063 PfPoint* land = new PfPoint(id, color, mobile, pt.coordX(), pt.coordY());
00064 land->link = &lms[i];
00065 landmarks.push_back(land);
00066 }
00067 else if (type == blobDataType) {
00068 const Shape<BlobData> &blob = ShapeRootTypeConst(lms[i],BlobData);
00069 id = blob->getId();
00070 Point pt = blob->getCentroid();
00071 PfBlob* land = new PfBlob(id, color, mobile, pt.coordX(), pt.coordY());
00072 land->link = &lms[i];
00073 landmarks.push_back(land);
00074 }
00075 }
00076 }
00077
00078 void PfRoot::deleteLms(std::vector<PfRoot*>& vec) {
00079 for ( unsigned int i = 0; i < vec.size(); i++ )
00080 delete vec[i];
00081 vec.clear();
00082 }
00083
00084 void PfRoot::findBounds(const std::vector<PfRoot*> &landmarks,
00085 coordinate_t &xmin, coordinate_t &ymin,
00086 coordinate_t &xmax, coordinate_t &ymax) {
00087 if ( landmarks.size() == 0 ) {
00088 cout << "Error in PFRoot::findBounds -- empty landmark vector" << endl;
00089 return;
00090 }
00091 xmin = xmax = landmarks[0]->x;
00092 ymin = ymax = landmarks[0]->y;
00093 for ( size_t i = 1; i<landmarks.size(); i++ ) {
00094 if ( (*landmarks[i]).x < xmin )
00095 xmin = landmarks[i]->x;
00096 else if ( landmarks[i]->x > xmax )
00097 xmax = landmarks[i]->x;
00098 if ( landmarks[i]->y < ymin )
00099 ymin = landmarks[i]->y;
00100 else if ( landmarks[i]->y > ymax )
00101 ymax = landmarks[i]->y;
00102 }
00103 }
00104
00105 void PfRoot::printLms(const std::vector<PfRoot*> &landmarks) {
00106 for (unsigned int i = 0; i<landmarks.size(); i++)
00107 cout << *landmarks[i] << endl;
00108 }
00109
00110 void PfRoot::printRootInfo(std::ostream &os) const {
00111 os << "id=" << id
00112 << ", x=" << x
00113 << ", y=" << y
00114 << ", mobile=" << mobile;
00115 }
00116
00117 ostream& operator<<(std::ostream &os, const PfRoot &obj) {
00118 obj.print(os);
00119 return os;
00120 }
00121
00122 void PfLine::print(std::ostream &os) const {
00123 os << "PfLine(";
00124 printRootInfo(os);
00125 os << ", x2=" << x2
00126 << ", y2=" << y2
00127 << ", length=" << length
00128 << ")";
00129 }
00130
00131 void PfEllipse::print(std::ostream &os) const {
00132 os << "PfEllipse(";
00133 printRootInfo(os);
00134 os << ")";
00135 }
00136
00137 void PfPoint::print(std::ostream &os) const {
00138 os << "PfPoint(";
00139 printRootInfo(os);
00140 os << ")";
00141 }
00142
00143 void PfBlob::print(std::ostream &os) const {
00144 os << "PfBlob(";
00145 printRootInfo(os);
00146 os << ")";
00147 }
00148
00149
00150 }