4 #include <visp3/core/vpColorDepthConversion.h>
5 #include <visp3/core/vpIoTools.h>
6 #include <visp3/core/vpMeterPixelConversion.h>
7 #include <visp3/core/vpXmlParserCamera.h>
10 #include <visp3/vision/vpPlaneEstimation.h>
11 #include <visp3/vision/vpPose.h>
14 #include <visp3/io/vpImageIo.h>
17 #include <visp3/gui/vpDisplayD3D.h>
18 #include <visp3/gui/vpDisplayGDI.h>
19 #include <visp3/gui/vpDisplayGTK.h>
20 #include <visp3/gui/vpDisplayOpenCV.h>
21 #include <visp3/gui/vpDisplayX.h>
24 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_PUGIXML)
30 #if defined(VISP_HAVE_X11)
32 #elif defined(VISP_HAVE_GDI)
34 #elif defined(VISP_HAVE_OPENCV)
36 #elif defined(VISP_HAVE_GTK)
38 #elif defined(VISP_HAVE_D3D9)
45 constexpr
auto ModelCommentHeader {
"#" };
46 constexpr
auto ModelKeypointsHeader {
"Keypoints" };
47 constexpr
auto ModelBoundsHeader {
"Bounds" };
48 constexpr
auto ModelDataHeader {
"data:" };
51 constexpr
auto DepthScale { 0.001 };
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
60 Model(
const Model &) =
default;
61 Model(Model &&) =
default;
62 Model &operator=(
const Model &) =
default;
63 Model &operator=(Model &&) =
default;
65 explicit Model(
const std::string &model_filename);
68 using Id =
unsigned long int;
69 static inline std::string to_string(
const Id &
id) {
return std::to_string(
id); };
75 std::map<Id, vpPoint> m_keypoints {};
76 std::map<Id, vpPoint> m_bounds {};
79 inline Model::Model(
const std::string &model_filename)
82 file.open(model_filename.c_str(), std::fstream::in);
84 std::string line {}, subs {};
85 bool in_model_bounds {
false };
86 bool in_model_keypoints {
false };
87 unsigned int data_curr_line { 0 };
88 unsigned int data_line_start_pos { 0 };
91 in_model_bounds =
false;
92 in_model_keypoints =
false;
94 data_line_start_pos = 0;
97 while (getline(file, line)) {
98 if (line.substr(0, std::string(ModelCommentHeader).size()) == ModelCommentHeader || line == ModelDataHeader) {
101 else if (line == ModelBoundsHeader) {
103 in_model_bounds =
true;
106 else if (line == ModelKeypointsHeader) {
108 in_model_keypoints =
true;
112 if (data_curr_line == 0) {
114 data_line_start_pos = (
unsigned int)line.find(
"[") + 1;
118 std::stringstream ss(line.substr(data_line_start_pos, line.find(
"]") - data_line_start_pos));
119 unsigned int data_on_curr_line = 0;
121 while (getline(ss, subs,
',')) {
122 oXYZ[data_on_curr_line++] = std::atof(subs.c_str());
124 if (in_model_bounds) {
125 m_bounds.try_emplace(data_curr_line, oXYZ[0], oXYZ[1], oXYZ[2]);
127 else if (in_model_keypoints) {
128 m_keypoints.try_emplace(data_curr_line, oXYZ[0], oXYZ[1], oXYZ[2]);
142 auto bounds = m_bounds;
143 std::for_each(begin(bounds), end(bounds), [&cMo](
auto &bound) { bound.second.project(cMo); });
150 auto keypoints = m_keypoints;
151 std::for_each(begin(keypoints), end(keypoints), [&cMo](
auto &keypoint) { keypoint.second.project(cMo); });
156 std::ostream &
operator<<(std::ostream &os,
const Model &model)
158 os <<
"-Bounds:" << std::endl;
159 for (
const auto &[
id, bound] : model.bounds()) {
161 os << std::setw(4) << std::setfill(
' ') <<
id <<
": "
162 << std::setw(6) << std::setfill(
' ') << bound.get_X() <<
", "
163 << std::setw(6) << std::setfill(
' ') << bound.get_Y() <<
", "
164 << std::setw(6) << std::setfill(
' ') << bound.get_Z() << std::endl;
168 os <<
"-Keypoints:" << std::endl;
169 for (
const auto &[
id, keypoint] : model.keypoints()) {
171 os << std::setw(4) << std::setfill(
' ') <<
id <<
": "
172 << std::setw(6) << std::setfill(
' ') << keypoint.get_X() <<
", "
173 << std::setw(6) << std::setfill(
' ') << keypoint.get_Y() <<
", "
174 << std::setw(6) << std::setfill(
' ') << keypoint.get_Z() << std::endl;
182 readData(
const std::string &input_directory,
const unsigned int cpt = 0)
184 char buffer[FILENAME_MAX];
185 std::stringstream ss;
186 ss << input_directory <<
"/color_image_%04d.jpg";
187 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
188 const std::string filename_color = buffer;
191 ss << input_directory <<
"/depth_image_%04d.bin";
192 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
193 const std::string filename_depth = buffer;
201 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
202 if (file_depth.is_open()) {
203 unsigned int height = 0, width = 0;
206 I_depth_raw.resize(height, width);
208 for (
auto i = 0u; i < height; i++) {
209 for (
auto j = 0u; j < width; j++) {
217 ss << input_directory <<
"/camera.xml";
226 ss << input_directory <<
"/depth_M_color.txt";
227 std::ifstream file_depth_M_color(ss.str().c_str(), std::ios::in | std::ios::binary);
230 depth_M_color.
load(file_depth_M_color);
232 return { I_color, I_depth_raw, color_param, depth_param, depth_M_color };
238 Display disp_color(color_img, 0, 0,
"Roi bounds", DispScaleType);
239 disp_color.display(color_img);
240 disp_color.flush(color_img);
242 std::vector<vpImagePoint> v_ip {};
245 disp_color.display(color_img);
246 auto disp_lane { 0 };
254 for (
auto j = 0u; j < v_ip.size(); j++) {
258 disp_color.flush(color_img);
267 if (v_ip.size() > 0) {
268 v_ip.erase(std::prev(end(v_ip)));
286 std::map<Model::Id, vpImagePoint> getKeypointsFromUser(
vpImage<vpRGBa> color_img,
const Model &model,
287 const std::string &parent_data)
290 Display disp_color(color_img, 0, 0,
"Keypoints", DispScaleType);
291 disp_color.display(color_img);
292 disp_color.flush(color_img);
295 vpImageIo::read(I_help, parent_data +
"/data/d435_box_keypoints_user_helper.png");
296 Display disp_help(I_help, disp_color.getWindowXPosition() + color_img.
getWidth(), disp_color.getWindowYPosition(),
297 "Keypoints [help]", DispScaleType);
298 disp_help.display(I_help);
299 disp_help.flush(I_help);
301 std::map<Model::Id, vpImagePoint> keypoints {};
311 for (
const auto &[
id, ip_unused] : model.keypoints()) {
314 disp_color.display(color_img);
315 auto disp_lane { 0 };
322 for (
const auto &[
id, keypoint] : keypoints) {
326 disp_color.flush(color_img);
331 keypoints.try_emplace(
id, ip);
339 int main(
int,
char *argv[])
341 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
343 #if defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_PUGIXML)
347 auto [color_img, depth_raw, color_param, depth_param, depth_M_color] =
352 std::cout <<
"color_param:" << std::endl << color_param << std::endl;
353 std::cout <<
"depth_param:" << std::endl << depth_param << std::endl;
354 std::cout <<
"depth_M_color:" << std::endl << depth_M_color << std::endl;
355 std::cout << std::endl <<
"Model:" << std::endl << model << std::endl;
358 Display display_color(color_img, 0, 0,
"Color", DispScaleType);
359 display_color.display(color_img);
360 display_color.flush(color_img);
364 Display display_depth(depth_img, display_color.getWindowXPosition() + display_color.getWidth(), 0,
"Depth",
366 display_depth.display(depth_img);
367 display_depth.flush(depth_img);
372 roi_color_img.
buildFrom(getRoiFromUser(color_img),
true);
374 std::vector<vpImagePoint> roi_corners_depth_img {};
376 cbegin(roi_color_img.getCorners()), cend(roi_color_img.getCorners()), std::back_inserter(roi_corners_depth_img),
381 depth_raw, DepthScale, 0.1, 0.6, depth_param, color_param, depth_M_color.inverse(), depth_M_color,
382 std::placeholders::_1));
383 const vpPolygon roi_depth_img { roi_corners_depth_img };
387 display_depth.flush(depth_img);
392 const auto obj_plane_in_depth =
393 vpPlaneEstimation::estimatePlane(depth_raw, DepthScale, depth_param, roi_depth_img, 1000, heat_map);
394 if (!obj_plane_in_depth) {
399 auto obj_plane_in_color = *obj_plane_in_depth;
400 obj_plane_in_color.changeFrame(depth_M_color.inverse());
403 Display display_heat_map(heat_map, display_depth.getWindowXPosition(),
404 display_depth.getWindowYPosition() + display_depth.getHeight(),
"Plane Estimation Heat map",
406 display_heat_map.display(heat_map);
407 display_heat_map.flush(heat_map);
410 const auto keypoint_color_img = getKeypointsFromUser(color_img, model,
vpIoTools::getParent(argv[0]));
413 const auto cMo = vpPose::computePlanarObjectPoseWithAtLeast3Points(obj_plane_in_color, model.keypoints(),
414 keypoint_color_img, color_param);
421 std::vector<vpImagePoint> d435_box_bound {};
432 for (
const auto &[id_unused, bound] : model.bounds(*cMo)) {
436 d435_box_bound.push_back(ip);
440 for (
const auto &[
id, keypoint] : model.keypoints(*cMo)) {
452 auto disp_lane { 0 };
462 std::cout <<
"There is no display and pugixml available to run this tutorial." << std::endl;
466 std::cout <<
"c++17 should be enabled to run this tutorial." << std::endl;
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
static vpImagePoint projectColorToDepth(const vpImage< uint16_t > &I_depth, double depth_scale, double depth_min, double depth_max, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics, const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
static const vpColor none
static const vpColor orange
static const vpColor blue
static const vpColor green
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1, bool closed=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void load(std::ifstream &f)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Defines a generic 2D polygon.
void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
XML parser to load and save intrinsic camera parameters.