Commit de31f3c4 authored by Pete Black's avatar Pete Black

reproject posed model and visualise resulting 2d screen coords

parent c8b0db7a
Pipeline #213493 failed with stages
in 1 minute and 17 seconds
......@@ -39,10 +39,12 @@
#include "tracking/t_tracking.h"
#include "tracking/t_helper_debug_sink.hpp"
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <inttypes.h>
#include <Eigen/Eigen>
#include "pose_db.hpp"
#include <pthread.h>
......@@ -443,7 +445,8 @@ optical_pose(struct exp_mono_hmd *emh,
std::cout << "POSE ROT: " << rotation_mat
<< "\nPOSE TRANS: " << translation_vec << "\n";
cv::Affine3f a((cv::Mat)rotation_mat, (cv::Mat)translation_vec);
*pose = Eigen::Matrix4f(a.matrix.val);
return true;
}
......@@ -1019,7 +1022,30 @@ void
reproject_pose(struct exp_mono_hmd *emh,
Eigen::Matrix4f pose,
std::vector<object_blob_t> *blobs)
{}
{
std::vector<cv::Point3f> points_to_reproject;
for (uint32_t i = 0; i < emh->model_rects.size(); i++) {
Eigen::Vector4f p(emh->model_rects[i].c.x,
emh->model_rects[i].c.y,
emh->model_rects[i].c.z, 0.0f);
Eigen::Vector4f transformed_pos = pose * p;
points_to_reproject.push_back(cv::Point3d(transformed_pos.x(),
transformed_pos.y(),
transformed_pos.z()));
}
std::vector<cv::Point2f> points_2d;
cv::Mat rvec(3, 1, CV_32FC1);
cv::Mat tvec(3, 1, CV_32FC1);
cv::projectPoints(points_to_reproject, rvec, tvec, emh->intrinsic,
emh->distortion, points_2d);
blobs->clear();
for (uint32_t i = 0; i < emh->model_rects.size(); i++) {
object_blob_t ob;
ob.blob.blob_id = i;
ob.blob.undist_rect_center = points_2d.at(i);
blobs->push_back(ob);
}
}
void
......@@ -1256,8 +1282,33 @@ process_frame(struct exp_mono_hmd *emh,
Eigen::Matrix4f pose;
if (ob_blobs.size() > 3) {
optical_pose(emh, &ob_blobs, &pose);
possible_poses.push_back(pose);
if (optical_pose(emh, &ob_blobs,
&pose)) {
std::vector<object_blob_t>
blobs;
reproject_pose(emh, pose,
&blobs);
if (emh->debug.rgb->cols > 0) {
for (uint32_t i = 0;
i < blobs.size();
i++) {
object_blob_t *b =
&blobs.at(
i);
cv::circle(
emh->debug
.rgb[0],
b->blob
.undist_rect_center,
10,
cv::Scalar(
0, 255,
0));
}
}
possible_poses.push_back(pose);
}
}
iter++;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment