VisualCorner::VisualCorner(const int _x, const int _y,
const float _distance,
const float _bearing,
boost::shared_ptr<VisualLine> l1, boost::shared_ptr<VisualLine> l2,
const float _t1, const float _t2,
boost::shared_ptr<NaoPose> _pose)
: VisualObject(CORNER_NO_IDEA_ID,_x, _y, _distance, _bearing),
pose(_pose),
possibleCorners(ConcreteCorner::concreteCorners().begin(),
ConcreteCorner::concreteCorners().end()),
cornerType(UNKNOWN), secondaryShape(UNKNOWN), line1(l1), line2(l2),
lines(), t1(_t1), t2(_t2),
// Technically the initialization of tBar and tStem is incorrect here for
// which we apologize. It's a hack, but the true values of tBar and tStem
// will get assigned in determineCornerShape which is right here in the
// constructor.
tBar(line1), tStem(line2),
angleBetweenLines(0), orientation(0), physicalOrientation(0)
{
lines.push_back(line1);
lines.push_back(line2);
determineCornerShape();
// Calculate and set the standard deviation of the measurements
setDistanceSD(cornerDistanceToSD(_distance));
setBearingSD(cornerBearingToSD(_bearing, _distance));
setAngleX( static_cast<float>(HALF_IMAGE_WIDTH - _x) /
static_cast<float>(HALF_IMAGE_WIDTH) *
MAX_BEARING_RAD);
setAngleY(static_cast<float>(HALF_IMAGE_HEIGHT - _y) /
static_cast<float>(HALF_IMAGE_HEIGHT) *
MAX_ELEVATION_RAD);
}
VisualCorner::VisualCorner(const int _x, const int _y,
const float _distance,
const float _bearing,
const VisualLine l1, const VisualLine l2,
const float _t1, const float _t2)
: VisualLandmark(_x, _y, _distance, _bearing), cornerType(UNKNOWN),
line1(l1), line2(l2), t1(_t1), t2(_t2),
// Technically the initialization of tBar and tStem is incorrect here for
// which we apologize. It's a hack, but the true values of tBar and tStem
// will get assigned in determineCornerShape which is right here in the
// constructor.
tBar(line1), tStem(line2),
angleBetweenLines(0) {
determineCornerShape();
// calculateAngle();
// TODO: remove
}