36 #include <visp3/core/vpConfig.h> 38 #if defined(VISP_HAVE_CATCH2) 39 #define CATCH_CONFIG_ENABLE_BENCHMARKING 40 #define CATCH_CONFIG_RUNNER 43 #include <visp3/core/vpIoTools.h> 44 #include <visp3/io/vpImageIo.h> 45 #include <visp3/mbt/vpMbGenericTracker.h> 49 #include <visp3/gui/vpDisplayX.h> 54 bool runBenchmark =
false;
56 template <
typename Type>
57 bool read_data(
const std::string &input_directory,
int cpt,
const vpCameraParameters &cam_depth,
61 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 62 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
63 "Template function supports only unsigned char and vpRGBa images!");
66 sprintf(buffer, std::string(input_directory +
"/Images/Image_%04d.pgm").c_str(), cpt);
67 std::string image_filename = buffer;
69 sprintf(buffer, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
70 std::string depth_filename = buffer;
72 sprintf(buffer, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
73 std::string pose_filename = buffer;
81 unsigned int depth_width = 0, depth_height = 0;
82 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
83 if (!file_depth.is_open())
88 I_depth.
resize(depth_height, depth_width);
89 pointcloud.resize(depth_height*depth_width);
91 const float depth_scale = 0.000030518f;
92 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
93 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
95 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
101 pointcloud[i*I_depth.
getWidth()+j] = pt3d;
105 std::ifstream file_pose(pose_filename.c_str());
106 if (!file_pose.is_open()) {
110 for (
unsigned int i = 0; i < 4; i++) {
111 for (
unsigned int j = 0; j < 4; j++) {
112 file_pose >> cMo[i][j];
120 TEST_CASE(
"Benchmark generic tracker",
"[benchmark]") {
122 std::vector<int> tracker_type(2);
128 const std::string configFileCam1 = input_directory + std::string(
"/Config/chateau.xml");
129 const std::string configFileCam2 = input_directory + std::string(
"/Config/chateau_depth.xml");
132 tracker.loadConfigFile(configFileCam1, configFileCam2);
134 tracker.loadModel(input_directory +
"/Models/chateau.cao", input_directory +
"/Models/chateau.cao");
145 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
150 std::vector<vpColVector> pointcloud;
153 tracker.getCameraParameters(cam_color, cam_depth);
156 depth_M_color[0][3] = -0.05;
157 tracker.setCameraTransformationMatrix(
"Camera2", depth_M_color);
160 std::vector<vpImage<unsigned char>> images;
161 std::vector<vpImage<uint16_t>> depth_raws;
162 std::vector<std::vector<vpColVector>> pointclouds;
163 std::vector<vpHomogeneousMatrix> cMo_truth_all;
165 for (
int i = 1; i <= 40; i++) {
166 if (read_data(input_directory, i, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
168 depth_raws.push_back(I_depth_raw);
169 pointclouds.push_back(pointcloud);
170 cMo_truth_all.push_back(cMo_truth);
174 for (
int i = 40; i >= 1; i--) {
175 if (read_data(input_directory, i, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
177 depth_raws.push_back(I_depth_raw);
178 pointclouds.push_back(pointcloud);
179 cMo_truth_all.push_back(cMo_truth);
185 std::vector<std::map<std::string, int>> mapOfTrackerTypes;
188 #if defined(VISP_HAVE_OPENCV) 194 std::vector<std::string> benchmarkNames = {
196 "Edge + Depth dense MBT",
197 #if defined(VISP_HAVE_OPENCV) 199 "KLT + depth dense MBT",
200 "Edge + KLT + depth dense MBT" 204 std::vector<bool> monoculars = {
207 #if defined(VISP_HAVE_OPENCV) 214 for (
size_t idx = 0; idx < mapOfTrackerTypes.size(); idx++) {
215 tracker.resetTracker();
216 tracker.setTrackerType(mapOfTrackerTypes[idx]);
218 tracker.loadConfigFile(configFileCam1, configFileCam2);
219 tracker.loadModel(input_directory +
"/Models/chateau.cao", input_directory +
"/Models/chateau.cao");
220 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
221 tracker.initFromPose(images.front(), cMo_truth_all.front());
223 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
224 mapOfWidths[
"Camera2"] = monoculars[idx] ? 0 : I_depth_raw.
getWidth();
225 mapOfHeights[
"Camera2"] = monoculars[idx] ? 0 : I_depth_raw.
getHeight();
228 #ifndef DEBUG_DISPLAY 229 BENCHMARK(benchmarkNames[idx].c_str())
236 tracker.setDisplayFeatures(
true);
239 tracker.initFromPose(images.front(), cMo_truth_all.front());
241 for (
size_t i = 0; i < images.size(); i++) {
243 const std::vector<vpColVector>& pointcloud_current = pointclouds[i];
252 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
253 mapOfImages[
"Camera1"] = &I_current;
255 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
256 mapOfPointclouds[
"Camera2"] = &pointcloud_current;
258 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
259 cMo = tracker.getPose();
262 tracker.display(I, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth,
vpColor::red, 3);
274 #ifndef DEBUG_DISPLAY 284 for (
unsigned int i = 0; i < 3; i++) {
285 t_err[i] = pose_est[i] - pose_truth[i];
286 tu_err[i] = pose_est[i+3] - pose_truth[i+3];
289 const double max_translation_error = 0.005;
290 const double max_rotation_error = 0.03;
291 CHECK(sqrt(t_err.sumSquare()) < max_translation_error);
292 CHECK(sqrt(tu_err.sumSquare()) < max_rotation_error);
298 int main(
int argc,
char *argv[])
300 Catch::Session session;
303 using namespace Catch::clara;
304 auto cli = session.cli()
307 (
"run benchmark comparing naive code with ViSP implementation");
313 session.applyCommandLine(argc, argv);
315 int numFailed = session.run();
VISP_EXPORT int wait(double t0, double t)
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static const vpColor none
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Real-time 6D object pose tracking using its CAD model.
static void flush(const vpImage< unsigned char > &I)
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
static void read(vpImage< unsigned char > &I, const std::string &filename)
Implementation of column vector and the associated operations.
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))
Implementation of a pose vector and operations on poses.
unsigned int getHeight() const
Definition of the vpImage class member functions.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)