void TracePlot::drawTile(QString key, const QRect &rect, range_t<off_t> sampleRange)
{
QImage image(rect.size(), QImage::Format_ARGB32);
image.fill(Qt::transparent);
QPainter painter(&image);
painter.setRenderHint(QPainter::Antialiasing, true);
auto firstSample = sampleRange.minimum;
auto length = sampleRange.length();
// Is it a 2-channel (complex) trace?
if (auto src = dynamic_cast<SampleSource<std::complex<float>>*>(sampleSource.get())) {
auto samples = src->getSamples(firstSample, length);
if (samples == nullptr)
return;
painter.setPen(Qt::red);
plotTrace(painter, rect, reinterpret_cast<float*>(samples.get()), length, 2);
painter.setPen(Qt::blue);
plotTrace(painter, rect, reinterpret_cast<float*>(samples.get())+1, length, 2);
// Otherwise is it single channel?
} else if (auto src = dynamic_cast<SampleSource<float>*>(sampleSource.get())) {
auto samples = src->getSamples(firstSample, length);
if (samples == nullptr)
return;
painter.setPen(Qt::green);
plotTrace(painter, rect, samples.get(), length, 1);
} else {
throw std::runtime_error("TracePlot::paintMid: Unsupported source type");
}
emit imageReady(key, image);
}
/**
* This is the function that plots the helicopter AND the object of interest,
* along with its normal.
*
* @param objectPosition Position of an object
* @param objectNormal Normal of an object
* @param quadPosition Position of the quad
* @param quadOrientation Roll, Pitch, and Yaw of the quad.
*/
void WorldPlotter::plotTopView(Point3f objectPosition, Point3f objectNormal,
Point3f quadPosition, Point3f quadOrientation) {
Mat plot = Mat::zeros(plot_size_y, plot_size_x, CV_8UC3);
plotAxes(plot);
// Plot Normal Vector
Point2i object_normal_p1, object_normal_p2;
object_normal_p1.x = objectPosition.x / real_size_x * plot_size_x / 1000.0f
+ plot_size_x / 2;
object_normal_p1.y = objectPosition.y / real_size_y * plot_size_y / 1000.0f
+ plot_size_y / 2;
object_normal_p2.x = object_normal_p1.x - 25 * objectNormal.x;
object_normal_p2.y = object_normal_p1.y - 25 * objectNormal.y;
object_trace.push_back(object_normal_p1);
plotTrace(plot, object_trace, object_color);
line(plot, object_normal_p1, object_normal_p2, normal_color,
normal_thickness);
rectangle(plot,
Point2i(object_normal_p1.x - object_size,
object_normal_p1.y - object_size),
Point2i(object_normal_p1.x + object_size,
object_normal_p1.y + object_size),
object_color, object_thickness);
float x, y, z;
x = quadPosition.x;
y = quadPosition.y;
z = quadPosition.z;
float roll, pitch, yaw;
roll = quadOrientation.x;
pitch = quadOrientation.y;
yaw = quadOrientation.z;
float q_x, q_y;
q_x = (x + 0.1 * cos(yaw)) / real_size_x * plot_size_x
+ plot_size_x / 2;
q_y = (y + 0.1* sin(yaw)) / real_size_y * plot_size_y
+ plot_size_y / 2;
x = x / real_size_x * plot_size_x + plot_size_x / 2;
y = y / real_size_y * plot_size_y + plot_size_y / 2;
quad_trace.push_back(Point2i(x, y));
plotTrace(plot, quad_trace, quad_color);
rectangle(plot, Point2i(x - object_size, y - object_size),
Point2i(x + object_size, y + object_size), quad_color,
object_thickness);
rectangle(plot, Point2i(q_x - 1, q_y - 1), Point2i(q_x + 1, q_y + 1),
quad_color, object_thickness);
line(plot, Point2i(x, y), Point2i(q_x, q_y), normal_color, normal_thickness);
putText(plot, "Object", Point2i(object_normal_p1.x, object_normal_p1.y - 10),
FONT_HERSHEY_PLAIN, 1, text_color);
putText(plot, "Quad", Point2i(x, y - 10), FONT_HERSHEY_PLAIN, 1, text_color);
putText(plot, "iron curtain", Point2i(plot_size_x / 2 - 37, plot_size_y - 9),
FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255));
objectPosition *= 0.001;
Vec3f d = cv::Vec3f(objectPosition) - cv::Vec3f(quadPosition);
Point3f distance = cv::Point3f(sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]),
0, 0);
Point3f new_yaw = cv::Point3f(0, 0, arcTan(objectNormal.x,
objectNormal.y));
float normalization = sqrt(objectNormal.x * objectNormal.x +
objectNormal.y * objectNormal.y);
float scale = -1.5;
float golden_x = objectPosition.x + scale * objectNormal.x / normalization;
float golden_y = objectPosition.y + scale * objectNormal.y / normalization;
//printf("%f %f \n", golden_x, golden_y);
golden_x = golden_x / real_size_x * plot_size_x + plot_size_x / 2;
golden_y = golden_y / real_size_y * plot_size_y + plot_size_y / 2;
cv::Point2i goldenPoint = cv::Point2i(golden_x, golden_y);
cv::circle(plot, goldenPoint, 2, Scalar(0, 0, 255), 2);
cv::Vector<Point3f> coordinates;
vector<string> labels;
coordinates.push_back(objectPosition);
labels.push_back("P.Object pos.");
coordinates.push_back(quadPosition);
labels.push_back("P.Quad pos.");
coordinates.push_back(quadOrientation);
labels.push_back("O.Quad orient.");
coordinates.push_back(distance);
labels.push_back("D.Distance");
coordinates.push_back(new_yaw);
labels.push_back("P.Goal Yaw");
plotCoordinates(NULL, coordinates, labels);
outputPlot = plot;
namedWindow("Top View Plot");
imshow("Top View Plot", plot);
}
