Skip to content

Commit c2ad0f0

Browse files
PetervDoorenPetervDooren
authored
Merge pull request #65 from tue-robotics/feature/vizualise-fitting-live
Live visualization tool
2 parents f1f1ca8 + ad8ce3f commit c2ad0f0

File tree

4 files changed

+245
-108
lines changed

4 files changed

+245
-108
lines changed

‎ed_sensor_integration/CMakeLists.txt‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -113,8 +113,11 @@ add_dependencies(ed_image_saver ${catkin_EXPORTED_TARGETS})
113113
add_executable(ed_segmenter tools/segmenter.cpp)
114114
target_link_libraries(ed_segmenter ed_kinect)
115115

116-
add_executable(ed_fitter tools/fitter_viz.cpp)
117-
target_link_libraries(ed_fitter ed_kinect)
116+
add_executable(ed_fitter_data tools/fitter_viz_data.cpp)
117+
target_link_libraries(ed_fitter_data ed_kinect)
118+
119+
add_executable(ed_fitter_live tools/fitter_viz_live.cpp)
120+
target_link_libraries(ed_fitter_live ed_kinect)
118121

119122
# ------------------------------------------------------------------------------------------------
120123
# TESTS

ed_sensor_integration/tools/fitter_viz.cpp renamed to ed_sensor_integration/include/ed_sensor_integration/tools/fitter_viz.h

Lines changed: 4 additions & 106 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,6 @@
1+
#ifndef ED_SENSOR_INTEGRATION_TOOLS_FITTER_VIZ_H_
2+
#define ED_SENSOR_INTEGRATION_TOOLS_FITTER_VIZ_H_
3+
14
#include <ed/entity.h>
25
#include <ed/world_model.h>
36

@@ -7,8 +10,6 @@
710

811
#include <opencv2/highgui/highgui.hpp>
912

10-
#include "ed_sensor_integration/tools/snapshot.h"
11-
1213

1314
// visualization parameters
1415
int canvas_width = 800; // pixels
@@ -24,14 +25,6 @@ cv::Scalar entity_colour(0, 255, 255); // yellow
2425
cv::Scalar fitted_colour(243, 192, 15); // blue
2526
cv::Scalar measurement_colour(161, 17, 187); // purple
2627

27-
/**
28-
* @brief usage, print how the executable should be used and explain the input
29-
*/
30-
void usage()
31-
{
32-
std::cout << "Usage: ed_fitter IMAGE-FILE-OR-DIRECTORY WORLDMODEL_NAME ENTITY_ID" << std::endl;
33-
}
34-
3528
/**
3629
* @brief drawLine, draw a line in 2D metric space on a canvas
3730
* @param canvas: canvas to draw the line to
@@ -168,99 +161,4 @@ cv::Mat visualizeFitting(EntityRepresentation2D entity, geo::Transform2 sensor_p
168161
return canvas;
169162
}
170163

171-
/**
172-
* @brief main executable to visualise the fitting process of stored images.
173-
* @param argc: should be 4
174-
* @return
175-
*/
176-
int main(int argc, char **argv)
177-
{
178-
if (argc != 4)
179-
{
180-
usage();
181-
return 1;
182-
}
183-
184-
std::string model_name = argv[2];
185-
186-
ed::WorldModelPtr world_model;
187-
try
188-
{
189-
world_model = ed::loadWorldModel(model_name);
190-
}
191-
catch (ed::ModelNotFoundException e)
192-
{
193-
std::cerr << "World model '" << model_name << "' could not be loaded." << std::endl;
194-
std::cerr << e.what() << std::endl;
195-
return 1;
196-
}
197-
198-
std::string entity_id = argv[3];
199-
ed::EntityConstPtr e = world_model->getEntity(entity_id);
200-
if (!e)
201-
{
202-
std::cerr << "Entity '" << entity_id << "' could not be found in world model '" << model_name << "'." << std::endl;
203-
return 1;
204-
}
205-
206-
tue::filesystem::Path path = argv[1];
207-
if (!path.exists())
208-
{
209-
std::cerr << "Path '" << path << "' does not exist." << std::endl;
210-
return 1;
211-
}
212-
213-
ed::SnapshotCrawler crawler(path);
214-
215-
Fitter fitter;
216-
217-
while(true)
218-
{
219-
ed::Snapshot& snapshot = crawler.current();
220-
221-
FitterData fitterdata;
222-
geo::Pose3D fitted_pose;
223-
224-
fitter.configureBeamModel(snapshot.image->getCameraModel());
225-
fitter.processSensorData(*snapshot.image, snapshot.sensor_pose, fitterdata);
226-
227-
bool estimateEntityPose = fitter.estimateEntityPose(fitterdata, *world_model, entity_id, e->pose(), fitted_pose);
228-
229-
// poses for visualization
230-
geo::Transform2 sensor_pose2d = fitterdata.sensor_pose_xya.projectTo2d();
231-
geo::Transform2 entity_pose2d = e->pose().projectTo2d();
232-
geo::Transform2 fitted_pose2d = fitted_pose.projectTo2d();
233-
234-
// show snapshot
235-
cv::Mat rgbcanvas = snapshot.image->getRGBImage().clone();
236-
cv::imshow("RGB", rgbcanvas);
237-
238-
EntityRepresentation2D entity_2d = fitter.GetOrCreateEntity2D(e);
239-
cv::Mat canvas = visualizeFitting(entity_2d, sensor_pose2d, entity_pose2d, fitted_pose2d, fitterdata, estimateEntityPose);
240-
cv::imshow("Fitting", canvas);
241-
char key = cv::waitKey();
242-
243-
if (key == 81) // Left arrow
244-
{
245-
crawler.previous();
246-
}
247-
else if (key == 83) // Right arrow
248-
{
249-
crawler.next();
250-
}
251-
else if (key == 82) // Up arrow
252-
{
253-
canvas_resolution = canvas_resolution + 10;
254-
}
255-
else if (key == 84) // Down arrow
256-
{
257-
canvas_resolution = std::max(canvas_resolution - 10.0, 10.0);
258-
}
259-
else if (key == 'q')
260-
{
261-
break;
262-
}
263-
}
264-
265-
return 0;
266-
}
164+
#endif // ED_SENSOR_INTEGRATION_TOOLS_FITTER_VIZ_H_

‎ed_sensor_integration/tools/fitter_viz_data.cpp‎

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
#include <ed/entity.h>
2+
#include <ed/world_model.h>
3+
4+
#include <ed/kinect/fitter.h>
5+
6+
#include <geolib/math_types.h>
7+
8+
#include <opencv2/highgui/highgui.hpp>
9+
10+
#include "ed_sensor_integration/tools/fitter_viz.h"
11+
#include "ed_sensor_integration/tools/snapshot.h"
12+
13+
14+
/**
15+
* @brief usage, print how the executable should be used and explain the input
16+
*/
17+
void usage()
18+
{
19+
std::cout << "Usage: ed_fitter IMAGE-FILE-OR-DIRECTORY WORLDMODEL_NAME ENTITY_ID" << std::endl;
20+
}
21+
22+
/**
23+
* @brief main executable to visualise the fitting process of stored images.
24+
* @param argc: should be 4
25+
* @return
26+
*/
27+
int main(int argc, char **argv)
28+
{
29+
if (argc != 4)
30+
{
31+
usage();
32+
return 1;
33+
}
34+
35+
tue::filesystem::Path path = argv[1];
36+
if (!path.exists())
37+
{
38+
std::cerr << "Path '" << path << "' does not exist." << std::endl;
39+
return 1;
40+
}
41+
ed::SnapshotCrawler crawler(path);
42+
43+
std::string model_name = argv[2];
44+
ed::WorldModelPtr world_model;
45+
try
46+
{
47+
world_model = ed::loadWorldModel(model_name);
48+
}
49+
catch (ed::ModelNotFoundException e)
50+
{
51+
std::cerr << "World model '" << model_name << "' could not be loaded." << std::endl;
52+
std::cerr << e.what() << std::endl;
53+
return 1;
54+
}
55+
56+
std::string entity_id = argv[3];
57+
ed::EntityConstPtr e = world_model->getEntity(entity_id);
58+
if (!e)
59+
{
60+
std::cerr << "Entity '" << entity_id << "' could not be found in world model '" << model_name << "'." << std::endl;
61+
return 1;
62+
}
63+
64+
Fitter fitter;
65+
66+
while(true)
67+
{
68+
ed::Snapshot& snapshot = crawler.current();
69+
70+
FitterData fitterdata;
71+
geo::Pose3D fitted_pose;
72+
73+
fitter.configureBeamModel(snapshot.image->getCameraModel());
74+
fitter.processSensorData(*snapshot.image, snapshot.sensor_pose, fitterdata);
75+
76+
bool estimateEntityPose = fitter.estimateEntityPose(fitterdata, *world_model, entity_id, e->pose(), fitted_pose);
77+
78+
// poses for visualization
79+
geo::Transform2 sensor_pose2d = fitterdata.sensor_pose_xya.projectTo2d();
80+
geo::Transform2 entity_pose2d = e->pose().projectTo2d();
81+
geo::Transform2 fitted_pose2d = fitted_pose.projectTo2d();
82+
83+
// show snapshot
84+
cv::Mat rgbcanvas = snapshot.image->getRGBImage();
85+
cv::imshow("RGB", rgbcanvas);
86+
87+
EntityRepresentation2D entity_2d = fitter.GetOrCreateEntity2D(e);
88+
cv::Mat canvas = visualizeFitting(entity_2d, sensor_pose2d, entity_pose2d, fitted_pose2d, fitterdata, estimateEntityPose);
89+
cv::imshow("Fitting", canvas);
90+
char key = cv::waitKey();
91+
92+
if (key == 81) // Left arrow
93+
{
94+
crawler.previous();
95+
}
96+
else if (key == 83) // Right arrow
97+
{
98+
crawler.next();
99+
}
100+
else if (key == 82) // Up arrow
101+
{
102+
canvas_resolution = canvas_resolution + 10;
103+
}
104+
else if (key == 84) // Down arrow
105+
{
106+
canvas_resolution = std::max(canvas_resolution - 10.0, 10.0);
107+
}
108+
else if (key == 'q')
109+
{
110+
break;
111+
}
112+
}
113+
cv::destroyAllWindows();
114+
return 0;
115+
}

‎ed_sensor_integration/tools/fitter_viz_live.cpp‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
#include <ed/entity.h>
2+
#include <ed/world_model.h>
3+
4+
#include <ed/kinect/fitter.h>
5+
#include <ed/kinect/image_buffer.h>
6+
7+
#include <geolib/math_types.h>
8+
9+
#include <opencv2/highgui/highgui.hpp>
10+
11+
#include "ed_sensor_integration/tools/fitter_viz.h"
12+
#include "ed_sensor_integration/tools/snapshot.h"
13+
14+
15+
/**
16+
* @brief usage, print how the executable should be used and explain the input
17+
*/
18+
void usage()
19+
{
20+
std::cout << "Usage: ed_fitter_live WORLDMODEL_NAME ENTITY_ID RGBD_TOPIC" << std::endl
21+
<< "WORLDMODEL_NAME name of the worldmodel to load, example: impuls" << std::endl
22+
<< "ENTITY_ID entity to fit, example dinner_table" << std::endl
23+
<< "RGBD_TOPIC topic on which the rgbd image is published, example /hero/head_rgbd_sensor/rgbd" << std::endl;
24+
}
25+
26+
/**
27+
* @brief main executable to visualise the fitting process of live images.
28+
* @param argc: should be 4
29+
* @return
30+
*/
31+
int main(int argc, char **argv)
32+
{
33+
// input processed. starting implementation
34+
ros::init(argc, argv, "fitting_visualizer");
35+
36+
if (argc != 4)
37+
{
38+
usage();
39+
return 1;
40+
}
41+
42+
std::string model_name = argv[1];
43+
44+
ed::WorldModelPtr world_model;
45+
try
46+
{
47+
world_model = ed::loadWorldModel(model_name);
48+
}
49+
catch (ed::ModelNotFoundException e)
50+
{
51+
std::cerr << "World model '" << model_name << "' could not be loaded." << std::endl;
52+
std::cerr << e.what() << std::endl;
53+
return 1;
54+
}
55+
56+
std::string entity_id = argv[2];
57+
ed::EntityConstPtr e = world_model->getEntity(entity_id);
58+
if (!e)
59+
{
60+
std::cerr << "Entity '" << entity_id << "' could not be found in world model '" << model_name << "'." << std::endl;
61+
return 1;
62+
}
63+
64+
std::string topic = argv[3];
65+
std::cout << "Using topic: " << topic << std::endl;
66+
67+
ImageBuffer image_buffer;
68+
image_buffer.initialize(topic, "map");
69+
70+
Fitter fitter;
71+
72+
while(ros::ok())
73+
{
74+
rgbd::ImageConstPtr image;
75+
geo::Pose3D sensor_pose;
76+
77+
if (!image_buffer.waitForRecentImage(image, sensor_pose, 2.0))
78+
{
79+
std::cerr << "No image received, will try again." << std::endl;
80+
continue;
81+
}
82+
83+
FitterData fitterdata;
84+
geo::Pose3D fitted_pose;
85+
86+
fitter.configureBeamModel(image->getCameraModel());
87+
fitter.processSensorData(*image, sensor_pose, fitterdata);
88+
89+
bool estimateEntityPose = fitter.estimateEntityPose(fitterdata, *world_model, entity_id, e->pose(), fitted_pose);
90+
91+
// poses for visualization
92+
geo::Transform2 sensor_pose2d = fitterdata.sensor_pose_xya.projectTo2d();
93+
geo::Transform2 entity_pose2d = e->pose().projectTo2d();
94+
geo::Transform2 fitted_pose2d = fitted_pose.projectTo2d();
95+
96+
// show snapshot
97+
cv::Mat rgbcanvas = image->getRGBImage();
98+
cv::imshow("RGB", rgbcanvas);
99+
100+
EntityRepresentation2D entity_2d = fitter.GetOrCreateEntity2D(e);
101+
cv::Mat canvas = visualizeFitting(entity_2d, sensor_pose2d, entity_pose2d, fitted_pose2d, fitterdata, estimateEntityPose);
102+
cv::imshow("Fitting", canvas);
103+
104+
char key = cv::waitKey(30);
105+
106+
if (key == 82) // Up arrow
107+
{
108+
canvas_resolution = canvas_resolution + 10;
109+
}
110+
else if (key == 84) // Down arrow
111+
{
112+
canvas_resolution = std::max(canvas_resolution - 10.0, 10.0);
113+
}
114+
else if (key == 'q')
115+
{
116+
break;
117+
}
118+
}
119+
cv::destroyAllWindows();
120+
return 0;
121+
}

0 commit comments

Comments
 (0)