From 17bdc29d5b3271056c9b4f85fff1a35e1bb920cf Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 26 Jun 2013 12:52:43 +0200 Subject: [PATCH] setShapePose and getShapePose implementations --- modules/viz/include/opencv2/viz/viz3d.hpp | 3 ++ modules/viz/src/q/viz3d_impl.hpp | 3 ++ modules/viz/src/viz3d.cpp | 10 +++++ modules/viz/src/viz3d_impl.cpp | 45 +++++++++++++++++++++++ modules/viz/src/viz_main.cpp | 7 ++++ modules/viz/test/test_viz3d.cpp | 19 +++++----- 6 files changed, 78 insertions(+), 9 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 0f5937a7bb..386d71141e 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -38,6 +38,9 @@ namespace temp_viz void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255)); void showCylinder(const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color = Color(255,255,255)); void showCircle(const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255)); + + Affine3f getShapePose(const String &id); + bool setShapePose(const String &id, const Affine3f &pose); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index e5a6cbaa15..29c3de6248 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -144,6 +144,8 @@ public: void showCube (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color); void showCylinder (const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color); void showCircle (const String &id, const Point3f &pt, double radius, const Color &color); + Affine3f getShapePose (const String &id); + bool setShapePose (const String &id, const Affine3f &pose); bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); @@ -422,6 +424,7 @@ private: //void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer &vtk_matrix); void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer &vtk_matrix); +void convertToCvMatrix (const vtkSmartPointer &vtk_matrix, cv::Matx44f &m); /** \brief Convert origin and orientation to vtkMatrix4x4 * \param[in] origin the point cloud origin diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 887d10062c..5aa0fe20c9 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -108,6 +108,16 @@ void temp_viz::Viz3d::showCircle(const String &id, const Point3f &pt, double rad impl_->showCircle(id, pt, radius, color); } +cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id) +{ + return impl_->getShapePose(id); +} + +bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose) +{ + return impl_->setShapePose(id, pose); +} + bool temp_viz::Viz3d::removeCoordinateSystem (const String &id) { return impl_->removeCoordinateSystem(id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 0c9718f57c..0a93e9c6b2 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -477,6 +477,51 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt, } } +cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String &id) +{ + // Get the shape with the id and return the pose + ShapeActorMap::iterator am_it = shape_actor_map_->find (id); + bool exists = (am_it != shape_actor_map_->end()); + + if (!exists) + { + std::cout << "[getShapePose] A shape with id " << id << " does not exist!" << std::endl; + return Affine3f(); + } + + vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second); + + vtkSmartPointer matrix = actor->GetUserMatrix(); + + Matx44f pose_mat; + convertToCvMatrix(matrix, pose_mat); + + Affine3f pose; + pose.matrix = pose_mat; + return pose; +} + +bool temp_viz::Viz3d::VizImpl::setShapePose (const String &id, const Affine3f &pose) +{ + ShapeActorMap::iterator am_it = shape_actor_map_->find (id); + bool exists = (am_it != shape_actor_map_->end()); + + if (!exists) + { + return false; + } + + vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second); + vtkSmartPointer matrix = vtkSmartPointer::New (); + + convertToVtkMatrix (pose.matrix, matrix); + + actor->SetUserMatrix (matrix); + actor->Modified (); + + return (true); +} + bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp index 7abb00d10c..a55247b7bf 100644 --- a/modules/viz/src/viz_main.cpp +++ b/modules/viz/src/viz_main.cpp @@ -1335,6 +1335,13 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointerSetElement (i, k, m (i, k)); } +void temp_viz::convertToCvMatrix (const vtkSmartPointer &vtk_matrix, cv::Matx44f &m) +{ + for (int i = 0; i < 4; i++) + for (int k = 0; k < 4; k++) + m(i,k) = vtk_matrix->GetElement (i, k); +} + ////////////////////////////////////////////////////////////////////////////////////////////// void temp_viz::convertToEigenMatrix (const vtkSmartPointer &vtk_matrix, Eigen::Matrix4f &m) { diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 1d09a16440..5307c4ad78 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -92,20 +92,21 @@ TEST(Viz_viz3d, accuracy) int col_blue = 0; int col_green = 0; int col_red = 0; + v.showCircle("circle1", cv::Point3f(0,0,0), fabs(1.0f), temp_viz::Color(0,255,0)); while(!v.wasStopped()) { // Creating new point cloud with id cloud1 cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z)); - v.showPointCloud("cloud1", cloud, temp_viz::Color(col_blue, col_green, col_red), cloudPosition); - v.showLine("line1", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); - v.showLine("line2", cv::Point3f(0.0,0.0,0.0), cv::Point3f(1.0f-pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); - v.showLine("line3", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, 1.0f-pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); - v.showLine("line4", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, 1.0f-pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); - v.showPlane("plane1", cv::Vec4f(pos_x*pos_y,pos_y,pos_z,pos_x+pos_y*pos_z), temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); - v.showCube("cube1", cv::Point3f(pos_x, pos_y, pos_z), cv::Point3f(pos_x+0.5, pos_y+0.5, pos_z+0.5), temp_viz::Color(255,150,50)); - v.showCylinder("cylinder1", cv::Point3f(0,0,0), cv::Point3f(pos_x, 1.0, 1.0), 0.5, 5*pos_x+3, temp_viz::Color(0,255,0)); - v.showCircle("circle1", cv::Point3f(0,0,0), fabs(pos_x), temp_viz::Color(0,255,0)); +// v.showPointCloud("cloud1", cloud, temp_viz::Color(col_blue, col_green, col_red), cloudPosition); +// v.showLine("line1", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); +// v.showLine("line2", cv::Point3f(0.0,0.0,0.0), cv::Point3f(1.0f-pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); +// v.showLine("line3", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, 1.0f-pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); +// v.showLine("line4", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, 1.0f-pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); +// v.showPlane("plane1", cv::Vec4f(pos_x*pos_y,pos_y,pos_z,pos_x+pos_y*pos_z), temp_viz::Color(255-col_blue, 255-col_green, 255-col_red)); +// v.showCube("cube1", cv::Point3f(pos_x, pos_y, pos_z), cv::Point3f(pos_x+0.5, pos_y+0.5, pos_z+0.5), temp_viz::Color(255,150,50)); +// v.showCylinder("cylinder1", cv::Point3f(0,0,0), cv::Point3f(pos_x, 1.0, 1.0), 0.5, 5*pos_x+3, temp_viz::Color(0,255,0)); + v.setShapePose("circle1", cloudPosition); angle_x += 0.1f; angle_y -= 0.1f; -- 2.34.1