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>
54bool runBenchmark =
false;
56template <
typename Type>
60#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
61 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
62 "Template function supports only unsigned char and vpRGBa images!");
64#if VISP_HAVE_DATASET_VERSION >= 0x030600
65 std::string ext(
"png");
67 std::string ext(
"pgm");
69 char buffer[FILENAME_MAX];
70 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/Images/Image_%04d." + ext).c_str(), cpt);
71 std::string image_filename = buffer;
73 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
74 std::string depth_filename = buffer;
76 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
77 std::string pose_filename = buffer;
85 unsigned int depth_width = 0, depth_height = 0;
86 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
87 if (!file_depth.is_open())
92 I_depth.
resize(depth_height, depth_width);
93 pointcloud.resize(depth_height * depth_width);
95 const float depth_scale = 0.000030518f;
96 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
97 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
99 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
105 pointcloud[i * I_depth.
getWidth() + j] = pt3d;
109 std::ifstream file_pose(pose_filename.c_str());
110 if (!file_pose.is_open()) {
114 for (
unsigned int i = 0; i < 4; i++) {
115 for (
unsigned int j = 0; j < 4; j++) {
116 file_pose >> cMo[i][j];
124TEST_CASE(
"Benchmark generic tracker",
"[benchmark]")
127 std::vector<int> tracker_type(2);
132 const std::string input_directory =
134 const std::string configFileCam1 = input_directory + std::string(
"/Config/chateau.xml");
135 const std::string configFileCam2 = input_directory + std::string(
"/Config/chateau_depth.xml");
138 const bool verbose =
false;
139 tracker.loadConfigFile(configFileCam1, configFileCam2, verbose);
141 tracker.loadModel(input_directory +
"/Models/chateau.cao", input_directory +
"/Models/chateau.cao", verbose);
152 tracker.loadModel(input_directory +
"/Models/cube.cao", verbose, T);
157 std::vector<vpColVector> pointcloud;
160 tracker.getCameraParameters(cam_color, cam_depth);
163 depth_M_color[0][3] = -0.05;
164 tracker.setCameraTransformationMatrix(
"Camera2", depth_M_color);
167 std::vector<vpImage<unsigned char> > images;
168 std::vector<vpImage<uint16_t> > depth_raws;
169 std::vector<std::vector<vpColVector> > pointclouds;
170 std::vector<vpHomogeneousMatrix> cMo_truth_all;
172 for (
int i = 1; i <= 40; i++) {
173 if (read_data(input_directory, i, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
175 depth_raws.push_back(I_depth_raw);
176 pointclouds.push_back(pointcloud);
177 cMo_truth_all.push_back(cMo_truth);
181 for (
int i = 40; i >= 1; i--) {
182 if (read_data(input_directory, i, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
184 depth_raws.push_back(I_depth_raw);
185 pointclouds.push_back(pointcloud);
186 cMo_truth_all.push_back(cMo_truth);
192 std::vector<std::map<std::string, int> > mapOfTrackerTypes;
193 mapOfTrackerTypes.push_back(
195 mapOfTrackerTypes.push_back(
197#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
198 mapOfTrackerTypes.push_back(
206 std::vector<std::string> benchmarkNames = {
208 "Edge + Depth dense MBT",
209#if defined(VISP_HAVE_OPENCV)
211 "KLT + depth dense MBT",
212 "Edge + KLT + depth dense MBT"
216 std::vector<bool> monoculars = {
219#if defined(VISP_HAVE_OPENCV)
226 for (
size_t idx = 0; idx < mapOfTrackerTypes.size(); idx++) {
227 tracker.resetTracker();
228 tracker.setTrackerType(mapOfTrackerTypes[idx]);
230 const bool verbose =
false;
231 tracker.loadConfigFile(configFileCam1, configFileCam2, verbose);
232 tracker.loadModel(input_directory +
"/Models/chateau.cao", input_directory +
"/Models/chateau.cao", verbose);
233 tracker.loadModel(input_directory +
"/Models/cube.cao", verbose, T);
234 tracker.initFromPose(images.front(), cMo_truth_all.front());
236 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
237 mapOfWidths[
"Camera2"] = monoculars[idx] ? 0 : I_depth_raw.
getWidth();
238 mapOfHeights[
"Camera2"] = monoculars[idx] ? 0 : I_depth_raw.
getHeight();
242 BENCHMARK(benchmarkNames[idx].c_str())
249 tracker.setDisplayFeatures(
true);
252 tracker.initFromPose(images.front(), cMo_truth_all.front());
254 for (
size_t i = 0; i < images.size(); i++) {
256 const std::vector<vpColVector> &pointcloud_current = pointclouds[i];
265 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
266 mapOfImages[
"Camera1"] = &I_current;
268 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
269 mapOfPointclouds[
"Camera2"] = &pointcloud_current;
271 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
272 cMo = tracker.getPose();
275 tracker.display(I, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
280 I, 40, 20, std::string(
"Nb features: " + std::to_string(tracker.getError().getRows())),
vpColor::red);
298 for (
unsigned int i = 0; i < 3; i++) {
299 t_err[i] = pose_est[i] - pose_truth[i];
300 tu_err[i] = pose_est[i + 3] - pose_truth[i + 3];
303 const double max_translation_error = 0.006;
304 const double max_rotation_error = 0.03;
305 CHECK(sqrt(t_err.sumSquare()) < max_translation_error);
306 CHECK(sqrt(tu_err.sumSquare()) < max_rotation_error);
312int main(
int argc,
char *argv[])
314 Catch::Session session;
317 using namespace Catch::clara;
318 auto cli = session.cli()
321 (
"run benchmark comparing naive code with ViSP implementation");
327 session.applyCommandLine(argc, argv);
329 int numFailed = session.run();
339int main() {
return EXIT_SUCCESS; }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor none
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static void display(const vpImage< unsigned char > &I)
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 flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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)
Definition of the vpImage class member functions.
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
unsigned int getHeight() const
Real-time 6D object pose tracking using its CAD model.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Implementation of a pose vector and operations on poses.
VISP_EXPORT int wait(double t0, double t)