void draw(){
ofPushMatrix();
ofSetColor(0);
drawAngle();
ofTranslate(140, 0);
drawGraph();
drawInfo();
ofTranslate(N + 20, 0);
drawHistogram();
ofPopMatrix();
}
void Constraint::draw()
{
if (m_scanConstraint) {
m_color = s_scanColor;
}else if (satisfied()) {
m_color = s_satisfiedColor;
}else {
m_color = s_unsatisfiedColor;
}
glColor3fv(m_color);
switch (m_type) {
case Invalid: break;
case Position: drawPosition(); break;
case Distance: drawDistance(); break;
case Angle: drawAngle(); break;
case Torsion: drawTorsion(); break;
}
}
void FindObjectOnPlane::find(
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::CameraInfo::ConstPtr& info_msg,
const pcl_msgs::ModelCoefficients::ConstPtr& polygon_3d_coefficient_msg)
{
cv::Mat object_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
image_geometry::PinholeCameraModel model;
pcl::ModelCoefficients::Ptr polygon_coefficients
(new pcl::ModelCoefficients);
pcl_conversions::toPCL(*polygon_3d_coefficient_msg, *polygon_coefficients);
jsk_recognition_utils::Plane::Ptr plane
(new jsk_recognition_utils::Plane(polygon_coefficients->values));
model.fromCameraInfo(info_msg);
std::vector<cv::Point> all_points;
for (int j = 0; j < object_image.rows; j++) {
for (int i = 0; i < object_image.cols; i++) {
if (object_image.at<uchar>(j, i) == 255) {
all_points.push_back(cv::Point(i, j));
}
}
}
cv::RotatedRect obb = cv::minAreaRect(all_points);
cv::Mat min_area_rect_image;
cv::cvtColor(object_image, min_area_rect_image, CV_GRAY2BGR);
cv::Point2f vertices2f[4];
obb.points(vertices2f);
cv::line(min_area_rect_image, vertices2f[0], vertices2f[1],
cv::Scalar(0, 0, 255), 4);
cv::line(min_area_rect_image, vertices2f[1], vertices2f[2],
cv::Scalar(0, 0, 255), 4);
cv::line(min_area_rect_image, vertices2f[2], vertices2f[3],
cv::Scalar(0, 0, 255), 4);
cv::line(min_area_rect_image, vertices2f[3], vertices2f[0],
cv::Scalar(0, 0, 255), 4);
cv::Rect bb = obb.boundingRect();
std::vector<cv::Point3f> search_points_3d;
std::vector<cv::Point2f> search_points_2d;
generateStartPoints(cv::Point2f(bb.x, bb.y),
model, polygon_coefficients,
search_points_3d, search_points_2d);
for (size_t i = 0; i < search_points_2d.size(); i++) {
cv::circle(min_area_rect_image, search_points_2d[i], 5, cv::Scalar(0, 255, 0), 1);
}
//for (size_t i = 0; i < search_points_3d.size(); i++) {
double min_area = DBL_MAX;
double min_angle;
cv::Point2f min_search_point;
double min_x;
double min_y;
for (size_t i = 0; i < search_points_2d.size(); i++) {
std::vector<double> angles;
std::vector<double> max_x;
std::vector<double> max_y;
generateAngles(object_image, search_points_2d[i], angles,
max_x, max_y,
model, plane);
// draw angles
for (size_t j = 0; j < angles.size(); j++) {
double area = drawAngle(min_area_rect_image, search_points_2d[i], angles[j],
max_x[j], max_y[j], model, plane, cv::Scalar(0, 255, 0));
if (area < min_area) {
min_area = area;
min_x = max_x[j];
min_y = max_y[j];
min_angle = angles[j];
min_search_point = search_points_2d[i];
}
}
}
drawAngle(min_area_rect_image, min_search_point, min_angle,
min_x, min_y, model, plane, cv::Scalar(0, 255, 255));
// convert the points into 3-D
pub_min_area_rect_image_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::BGR8,
min_area_rect_image).toImageMsg());
NODELET_INFO("published");
}
