void color_write_image(matrix<double> &grid, const colormap_func &cmap, const std::string &output_filename, bool write_save) {
/* modifies argument! */
scale_grid(grid);
image_RGB color_image(grid.x(), grid.y());
grayscale_to_rgb(grid, color_image, cmap);
if (write_save) {
std::cout << "saving image" << std::endl;
}
write_image(color_image, output_filename);
}
void prepareMeasurement(const ed::EntityConstPtr& e, cv::Mat& view_color_img, cv::Mat& view_depth_img, cv::Mat& mask, cv::Rect& bouding_box) {
// Get the best measurement from the entity
ed::MeasurementConstPtr msr = e->lastMeasurement();
if (!msr)
return;
int min_x, max_x, min_y, max_y;
// create a view
rgbd::View view(*msr->image(), msr->image()->getRGBImage().cols);
// get depth image
view_depth_img = msr->image()->getDepthImage();
// get color image
const cv::Mat& color_image = msr->image()->getRGBImage();
// crop it to match the view
view_color_img = cv::Mat(color_image(cv::Rect(0, 0, view.getWidth(), view.getHeight())));
// std::cout << "image: " << cropped_image.cols << "x" << cropped_image.rows << std::endl;
// initialize bounding box points
max_x = 0;
max_y = 0;
min_x = view.getWidth();
min_y = view.getHeight();
// initialize mask, all 0s
mask = cv::Mat::zeros(view.getHeight(), view.getWidth(), CV_8UC1);
// Iterate over all points in the mask
for(ed::ImageMask::const_iterator it = msr->imageMask().begin(view.getWidth()); it != msr->imageMask().end(); ++it)
{
// mask's (x, y) coordinate in the depth image
const cv::Point2i& p_2d = *it;
// paint a mask
mask.at<unsigned char>(*it) = 255;
// update the boundary coordinates
if (min_x > p_2d.x) min_x = p_2d.x;
if (max_x < p_2d.x) max_x = p_2d.x;
if (min_y > p_2d.y) min_y = p_2d.y;
if (max_y < p_2d.y) max_y = p_2d.y;
}
bouding_box = cv::Rect(min_x, min_y, max_x - min_x, max_y - min_y);
}
bool ODUFinderModule::extractImage(const ed::Entity& e, cv::Mat& img) const
{
// Get the best measurement from the entity
ed::MeasurementConstPtr msr = e.lastMeasurement();
if (!msr)
return false;
// get color image
const cv::Mat& color_image = msr->image()->getRGBImage();
// Created masked image
cv::Rect rgb_roi;
ed::perception::maskImage(color_image, msr->imageMask(), rgb_roi);
// Create cropped masked color image
cv::Mat cropped_image = color_image(rgb_roi);
// convert to grayscale and increase contrast
cv::cvtColor(cropped_image, img, CV_BGR2GRAY);
cv::equalizeHist(croped_mono_image, img);
}
int main()
{
vpImage<unsigned char> gray_image(240, 320);
vpImage<vpRGBa> color_image(240, 320);
gray_image = 128;
vpRGBa color(255, 0, 0);
color_image = color;
unsigned int igray_max = gray_image.getHeight() - 1;
unsigned int jgray_max = gray_image.getWidth() - 1;
std::cout << "Gray image, last pixel intensity: "
<< (int)gray_image[igray_max][jgray_max] << std::endl;
unsigned int icolor_max = color_image.getHeight() - 1;
unsigned int jcolor_max = color_image.getWidth() - 1;
std::cout << "Color image, last pixel RGB components: "
<< (int)color_image[icolor_max][jcolor_max].R << " "
<< (int)color_image[icolor_max][jcolor_max].G << " "
<< (int)color_image[icolor_max][jcolor_max].B
<< std::endl;
}
void FaceDetector::writeFaceDetectionResult(const ed::Measurement& msr, const cv::Rect& rgb_roi,
const std::vector<cv::Rect>& rgb_face_rois,
int& face_counter, tue::Configuration& result) const
{
// get color image
const cv::Mat& color_image = msr.image()->getRGBImage();
// get color image
const cv::Mat& depth_image = msr.image()->getDepthImage();
// Calculate size factor between depth and rgb images
double f_depth_rgb = (double)depth_image.cols / color_image.cols;
// Create depth view
rgbd::View depth_view(*msr.image(), depth_image.cols);
for (uint j = 0; j < rgb_face_rois.size(); j++)
{
cv::Rect rgb_face_roi = rgb_face_rois[j];
rgb_face_roi.x += rgb_roi.x;
rgb_face_roi.y += rgb_roi.y;
if (debug_mode_)
ed::perception::saveDebugImage("face_detector-rgb", color_image(rgb_face_roi));
result.addArrayItem();
result.setValue("index", face_counter);
// add 2D location of the face
result.setValue("x", rgb_face_roi.x);
result.setValue("y", rgb_face_roi.y);
result.setValue("width", rgb_face_roi.width);
result.setValue("height", rgb_face_roi.height);
// Compute face roi for depth image
cv::Rect depth_face_roi(f_depth_rgb * rgb_face_roi.x, f_depth_rgb * rgb_face_roi.y,
f_depth_rgb * rgb_face_roi.width, f_depth_rgb * rgb_face_roi.height);
if (debug_mode_)
ed::perception::saveDebugImage("face_detector-depth", depth_image(depth_face_roi));
cv::Mat face_area = depth_image(depth_face_roi);
float avg_depth = ed::perception::getMedianDepth(face_area);
if (avg_depth > 0)
{
// calculate the center point of the face
cv::Point2i p_2d(depth_face_roi.x + depth_face_roi.width/2,
depth_face_roi.y + depth_face_roi.height/2);
geo::Vector3 projection = depth_view.getRasterizer().project2Dto3D(p_2d.x, p_2d.y) * avg_depth;
geo::Vector3 point_map = msr.sensorPose() * projection;
// add 3D location of the face
result.setValue("map_x", point_map.x);
result.setValue("map_y", point_map.y);
result.setValue("map_z", point_map.z);
}
else
{
std::cout << "[ED FACE DETECTOR] Could not calculate face's average depth. Map coordinates might be incorrect!" << std::endl;
}
result.endArrayItem();
face_counter++;
}
}
void ODUFinderModule::process(const ed::perception::WorkerInput& input, ed::perception::WorkerOutput& output) const
{
const ed::EntityConstPtr& e = input.entity;
tue::Configuration& result = output.data;
ed::ErrorContext errc("Processing entity in ODUFinderModule");
// output.type_update.setUnknownScore(type_unknown_score_);
if (!initialized_)
return;
// Get the best measurement from the entity
ed::MeasurementConstPtr msr = e->lastMeasurement();
if (!msr)
return;
// ----------------------- PREPARE IMAGE -----------------------
// get color image
const cv::Mat& color_image = msr->image()->getRGBImage();
// Created masked image
cv::Rect rgb_roi;
cv::Mat masked_color_image = ed::perception::maskImage(color_image, msr->imageMask(), rgb_roi);
// Create cropped masked color image
cv::Mat cropped_image = color_image(rgb_roi);
// convert to grayscale and increase contrast
cv::Mat croped_mono_image;
cv::cvtColor(cropped_image, croped_mono_image, CV_BGR2GRAY);
cv::equalizeHist(croped_mono_image, croped_mono_image);
// ----------------------- PROCESS IMAGE -----------------------
IplImage img(croped_mono_image);
std::map<std::string, float> results;
{
boost::lock_guard<boost::mutex> lg(mutex_update_);
results = odu_finder_->process_image(&img);
}
// ----------------------- ASSERT RESULTS -----------------------
// create group if it doesnt exist
if (!result.readGroup("perception_result", tue::OPTIONAL))
{
result.writeGroup("perception_result");
}
result.writeGroup("odu_finder");
// output.type_update.setUnknownScore(type_unknown_score_);
double total_score = 0;
// if an hypothesis is found, assert it
if (!results.empty())
{
for(std::map<std::string, float>::const_iterator it = results.begin(); it != results.end(); ++it)
{
double score = it->second * score_factor_;
output.type_update.setScore(it->first, score);
total_score += score;
}
}
if (total_score < 1.0)
output.type_update.setUnknownScore(1.0 - total_score);
else
output.type_update.setUnknownScore(0);
result.endGroup(); // close odu_finder group
result.endGroup(); // close perception_result group
if (debug_mode_){
cv::imwrite(debug_folder_ + ed::Entity::generateID().str() + "_odu_finder_module.png", croped_mono_image);
}
}
