mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-29 13:47:32 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

coordinate system widget implementation, update pose fix

Browse Source
This commit is contained in:
ozantonkal 2013-07-04 18:19:06 +03:00
parent 03cc439b08
commit ecdd8513fd
4 changed files with 66 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
modules/viz/include/opencv2/viz/widgets.hpp
View File

@ -79,5 +79,11 @@ namespace temp_viz
public:
CubeWidget(const Point3f& pt_min, const Point3f& pt_max, const Color &color = Color::white());
};
class CV_EXPORTS CoordinateSystemWidget : public Widget
{
public:
CoordinateSystemWidget(double scale, const Affine3f& affine);
};
}

50
modules/viz/src/simple_widgets.cpp
View File

@ -1,4 +1,5 @@
#include "precomp.hpp"
#include <opencv2/calib3d.hpp>
///////////////////////////////////////////////////////////////////////////////////////////////
/// line widget implementation
@ -225,3 +226,52 @@ temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, c
setColor(color);
}
///////////////////////////////////////////////////////////////////////////////////////////////
/// coordinate system widget implementation
temp_viz::CoordinateSystemWidget::CoordinateSystemWidget(double scale, const Affine3f& affine)
{
vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New ();
axes->SetOrigin (0, 0, 0);
axes->SetScaleFactor (scale);
vtkSmartPointer<vtkFloatArray> axes_colors = vtkSmartPointer<vtkFloatArray>::New ();
axes_colors->Allocate (6);
axes_colors->InsertNextValue (0.0);
axes_colors->InsertNextValue (0.0);
axes_colors->InsertNextValue (0.5);
axes_colors->InsertNextValue (0.5);
axes_colors->InsertNextValue (1.0);
axes_colors->InsertNextValue (1.0);
vtkSmartPointer<vtkPolyData> axes_data = axes->GetOutput ();
axes_data->Update ();
axes_data->GetPointData ()->SetScalars (axes_colors);
vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
axes_tubes->SetInput (axes_data);
axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0);
axes_tubes->SetNumberOfSides (6);
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
mapper->SetScalarModeToUsePointData ();
mapper->SetInput(axes_tubes->GetOutput ());
vtkSmartPointer<vtkLODActor> actor = WidgetAccessor::getActor(*this);
actor->SetMapper(mapper);
cv::Vec3d t = affine.translation();
actor->SetPosition (t[0], t[1], t[2]);
cv::Matx33f m = affine.rotation();
cv::Vec3f rvec;
cv::Rodrigues(m, rvec);
float r_angle = cv::norm(rvec);
rvec *= 1.f/r_angle;
actor->SetOrientation(0,0,0);
actor->RotateWXYZ(r_angle*180/CV_PI,rvec[0], rvec[1], rvec[2]);
}

5
modules/viz/src/widget.cpp
View File

@ -31,6 +31,11 @@ public:
void updatePose(const Affine3f& pose)
{
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
if (!matrix)
{
setPose(pose);
return ;
}
Matx44f matrix_cv = convertToMatx(matrix);
Affine3f updated_pose = pose * Affine3f(matrix_cv);

18
modules/viz/test/test_viz3d.cpp
View File

@ -73,8 +73,6 @@ TEST(Viz_viz3d, accuracy)
v.setBackgroundColor();
v.addCoordinateSystem(1.0, cv::Affine3f::Identity());
cv::Mat cloud = cvcloud_load();
cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0));
@ -91,7 +89,7 @@ TEST(Viz_viz3d, accuracy)
int col_blue = 0;
int col_green = 0;
int col_red = 0;
// v.showCircle("circle1", cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
temp_viz::LineWidget lw(cv::Point3f(0.0,0.0,0.0), cv::Point3f(4.0,4.0,4.0), temp_viz::Color(0,255,0));
temp_viz::PlaneWidget pw(cv::Vec4f(0.0,1.0,2.0,3.0));
temp_viz::SphereWidget sw(cv::Point3f(0,0,0), 0.5);
@ -99,6 +97,7 @@ TEST(Viz_viz3d, accuracy)
temp_viz::CircleWidget cw(cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
temp_viz::CylinderWidget cyw(cv::Point3f(0,0,0), cv::Point3f(-1,-1,-1), 0.5, 30, temp_viz::Color(0,255,0));
temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1), temp_viz::Color(0,0,255));
temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity());
v.showWidget("line", lw);
v.showWidget("plane", pw);
@ -107,6 +106,7 @@ TEST(Viz_viz3d, accuracy)
v.showWidget("circle", cw);
v.showWidget("cylinder", cyw);
v.showWidget("cube", cuw);
v.showWidget("coordinateSystem", csw);
temp_viz::LineWidget lw2 = lw;
@ -114,15 +114,7 @@ TEST(Viz_viz3d, accuracy)
{
// 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.setShapePose("circle1", cloudPosition);
lw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
lw.setLineWidth(lw.getLineWidth()+pos_x * 10);
@ -134,7 +126,7 @@ TEST(Viz_viz3d, accuracy)
cw.setPose(cloudPosition);
cyw.setPose(cloudPosition);
lw.setPose(cloudPosition);
cuw.setPose(cloudPosition);
cuw.setPose(cloudPosition);
angle_x += 0.1f;
angle_y -= 0.1f;