From 5384af65f9d63d095cb9987d36de57ccee323300 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 15:47:07 -0600 Subject: Added reproject tool --- tools/showreproject/Makefile | 16 +++ tools/showreproject/showreproject.cc | 271 +++++++++++++++++++++++++++++++++++ 2 files changed, 287 insertions(+) create mode 100644 tools/showreproject/Makefile create mode 100644 tools/showreproject/showreproject.cc (limited to 'tools') diff --git a/tools/showreproject/Makefile b/tools/showreproject/Makefile new file mode 100644 index 0000000..f6d5925 --- /dev/null +++ b/tools/showreproject/Makefile @@ -0,0 +1,16 @@ +all : showreproject + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE #-fsanitize=address -fsanitize=undefined +LDFLAGS:=-lm -lpthread -llapacke -lcblas -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_imgcodecs + +showreproject : showreproject.cc $(LIBSURVIVE) + cd ../..;make + g++ $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean : + rm -rf showreproject + diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc new file mode 100644 index 0000000..89d67c4 --- /dev/null +++ b/tools/showreproject/showreproject.cc @@ -0,0 +1,271 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "opencv2/imgproc.hpp" +#include +#include + +uint32_t timestamp; + +cv::Mat_ img; +cv::Mat_ err[2]; + +cv::Vec3f flow2rgb(float x, float y, float scale = 1) { + cv::Mat_ hsv(1, 1); + hsv(0, 0) = {atan2(y, x) * 180. / M_PI, 1.0, std::min(1.0f, sqrt(x * x + y * y) * scale)}; + cv::Mat_ bgr(1, 1); + cv::cvtColor(hsv, bgr, CV_HSV2RGB_FULL); + return bgr(0, 0) * 255; +} + +static void draw_viz(cv::Mat &img, double scale = 1) { + double size = 50; + cv::Point2f origin(size, size); + for (double r = size; r > size / 100.; r -= size / 100.) { + for (double theta = 0; theta < 2 * M_PI; theta += .01) { + auto x = cos(theta) * r; + auto y = sin(theta) * r; + auto clr = flow2rgb(x, y, scale / size); + cv::line(img, cv::Point2f(x, y) + origin, origin, clr); + } + } +} + +double error = 0; +int error_count = 0; + +static void redraw(SurviveContext *ctx) { + int SIZE = 1000; + int shift = ctx->user_ptr ? 1 : 0; + + auto show_pov = 130. / 180; + auto fov = 120. / 180. * SIZE / show_pov; + + auto map = [&](const double *a) { + auto x = a[0] * SIZE / (M_PI) / show_pov + SIZE / 2; + return cv::Point(x, SIZE - (a[1] * SIZE / (M_PI) + SIZE / 2)); + }; + + auto region = img.data ? img(cv::Rect(SIZE * shift, 0, SIZE, SIZE)) : cv::Mat_(); + + if (region.data) { + + region.setTo(cv::Vec3b(0, 0, 0)); + cv::rectangle(region, cv::Point(SIZE / 2 - fov / 2, SIZE / 2 - fov / 2), + cv::Point(SIZE / 2 + fov / 2, SIZE / 2 + fov / 2), cv::Vec3b(255, 255, 255)); + } + + // eregion.copyTo(region); + + for (int i = 0; i < ctx->objs_ct; i++) { + auto so = ctx->objs[i]; + auto scene = &so->activations; + for (size_t lh = 0; lh < 2; lh++) { + auto eregion = err[lh](cv::Rect(SIZE * shift, 0, SIZE, SIZE)); + + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + + auto ncolor = cv::Vec3b(lh == 0 ? 255 : 0, + 255, // sensor / (double)so->sensor_ct * 255, + i / (double)ctx->objs_ct * 255); + auto rcolor = cv::Vec3b(lh == 0 ? 255 : 0, + 128, // sensor / (double)so->sensor_ct * 255, + i / (double)ctx->objs_ct * 255); + + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timestamp, + sensor, lh)) { + const double *a = scene->angles[sensor][lh]; + // FLT a[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _a, a); + + auto l = scene->lengths[sensor][lh]; + double r = std::max(1., (l[0] + l[1]) / 1000.); + // std::cerr << lh << "\t" << sensor << "\t" << ((l[0] + l[1]) / 2000.) << "\t" << l[0] << "\t" << + // l[1] << std::endl; + if (region.data) + cv::circle(region, map(a), r, ncolor); + + FLT point3d[3]; + FLT out[2]; + ApplyPoseToPoint(point3d, &so->OutPose, so->sensor_locations + 3 * sensor); + survive_reproject(ctx, lh, point3d, out); + + double ex = out[0] - a[0]; + double ey = out[1] - a[1]; + double err_add = sqrt(ex * ex + ey * ey); + error += err_add; + error_count++; + + auto &e = eregion(map(a).y, map(a).x); + e = flow2rgb(ex, ey, 100); + + cv::putText(img, std::to_string(error / error_count), cv::Point2f(10, 20), CV_FONT_HERSHEY_PLAIN, 1, + cv::Scalar(255, 255, 255)); + + if (region.data) { + cv::line(region, map(a) + cv::Point(ex * 10000, -ey * 10000), map(a), + cv::Scalar(255, 255, 255)); + cv::rectangle(region, map(out) - cv::Point(r, r), map(out) + cv::Point(r, r), rcolor); + } + } + } + }; + } + if (img.data) { + cv::imshow("Reprojection", img); + } +} + +void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lighthouse) { + timestamp = timecode; + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); +} + +SurvivePose lastPose = {}; + +void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + survive_default_raw_pose_process(so, lighthouse, pose); + auto d = dist3d(lastPose.Pos, pose->Pos); + // std::cerr << d << std::endl; + if (d < .01) { + redraw(so->ctx); + } + lastPose = *pose; +} + +void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose); +} + +SurviveContext *create(int argc, char **argv) { + auto ctx = survive_init(argc, argv); + + survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_lighthouse_pose_fn(ctx, lighthouse_process); + survive_install_light_fn(ctx, light_process); + + return ctx; +} + +void drawbsds(SurviveContext *ctx) { + int SIZE = 1000; + + std::vector show_flags = { + SVCal_All, SVCal_Phase, SVCal_Gib, SVCal_Curve, SVCal_Tilt, + }; + + for (auto f : show_flags) { + for (int lh = 0; lh < 2; lh++) { + cv::Mat_ img = cv::Mat_(SIZE, SIZE); + img.setTo(cv::Vec3b(0, 0, 0)); + for (int x = 0; x < SIZE; x++) { + for (int y = 0; y < SIZE; y++) { + FLT in[2] = {x * M_PI / SIZE - M_PI / 2., y * M_PI / SIZE - M_PI / 2.}; + if (fabs(in[0]) > 60. / 180 * M_PI || fabs(in[1]) > 60. / 180 * M_PI) + continue; + + FLT out[2]; + auto config = survive_calibration_config_ctor(); + config.use_flag = f; + survive_apply_bsd_calibration_by_flag(ctx, lh, &config, in, out); + double ex = out[0] - in[0]; + double ey = out[1] - in[1]; + if (f == SVCal_All) { + ex -= ctx->bsd[lh].fcal.phase[0]; + ey -= ctx->bsd[lh].fcal.phase[1]; + } + + // Make it opposite of angles + ex *= -1; + ey *= -1; + + img(y, x) = flow2rgb(ex, ey, 100); + } + } + draw_viz(img); + cv::imwrite("BSD" + std::to_string(lh) + "_" + std::to_string(f) + ".png", img); + if (f == SVCal_All) + cv::imshow("BSD" + std::to_string(lh), img); + } + } +} + +int main(int argc, char **argv) { + // for (int i = 0; i < 1 << 5; i++) { + // size_t cidx = 15 | (i << 5); + + // auto conf = survive_calibration_config_create_from_idx(cidx); + // if (survive_calibration_config_index(&conf) != cidx) + // continue; + + auto ctx1 = create(argc, argv); + size_t cidx = survive_configi(ctx1, "default-cal-conf", SC_GET, 0); + size_t idx = survive_configi(ctx1, "default-cal-conf2", SC_GET, 0); + + SurviveContext *ctx2 = 0; + int numCtx = 1; + int ctx2_flag = 1; + if (idx != 0) { + numCtx++; + ctx2 = create(argc, argv); + ctx2->user_ptr = (void *)&ctx2_flag; + } + + size_t showui = survive_configi(ctx1, "show-ui", SC_GET, 0); + + drawbsds(ctx1); + + int waitUpdate = 100; + int SIZE = 1000; + if (showui) { + img = cv::Mat_(SIZE, numCtx * SIZE); + img.setTo(cv::Vec3b(0, 0, 0)); + } + for (int lh = 0; lh < 2; lh++) { + err[lh] = cv::Mat_(SIZE, numCtx * SIZE); + err[lh].setTo(cv::Vec3b(0, 0, 0)); + } + + if (img.data && false) { + cv::imshow("Reprojection", img); + cv::waitKey(0); + } + + auto start = std::chrono::high_resolution_clock::now(); + while (survive_poll(ctx1) == 0 && (ctx2 == 0 || survive_poll(ctx2) == 0)) { + auto now = std::chrono::high_resolution_clock::now(); + if ((now - start) > std::chrono::milliseconds(33)) { + cv::waitKey(1); + start = now; + } + } + + for (int i = 0; i < 2; i++) { + draw_viz(err[i]); + cv::putText(err[i], std::to_string(error / error_count), cv::Point2f(100, 20), CV_FONT_HERSHEY_PLAIN, 1, + cv::Scalar(255, 255, 255)); + cv::putText(err[i], std::to_string(cidx), cv::Point2f(100, 40), CV_FONT_HERSHEY_PLAIN, 1, + cv::Scalar(255, 255, 255)); + + auto name = "LH" + std::to_string(i); + cv::imwrite(name + "_" + std::to_string(cidx) + ".png", err[i]); + cv::imshow(name, err[i]); + } + + std::cerr << "Error: " << error / error_count << std::endl; + + int c = '\0'; + while (((c = cv::waitKey(0)) & 0xff) != 'q') { + std::cerr << (uint8_t)c << std::endl; + } + return 0; +} -- cgit v1.2.3