Skip to content

O
opencv
Commit c0f44ca4 authored by ozantonkal's avatar ozantonkal
Browse files
Options

shorten Trajectory widget implementation

parent 5af739df
Hide whitespace changes
Inline Side-by-side
Showing with 99 additions and 78 deletions
modules/viz/include/opencv2/viz/widgets.hpp
View file @ c0f44ca4
......@@ -180,6 +180,9 @@ namespace cv
public:
TrajectoryWidget(const std::vector<Affine3f> &path, const Color &color = Color::white(), bool show_frames = false, double scale = 1.0);
TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
private:
struct ApplyPath;
};
class CV_EXPORTS CloudWidget : public Widget3D
......
modules/viz/src/shape_widgets.cpp
View file @ c0f44ca4
......@@ -984,6 +984,36 @@ cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec2f &fov, double sca
///////////////////////////////////////////////////////////////////////////////////////////////
/// trajectory widget implementation
struct cv::viz::TrajectoryWidget::ApplyPath
{
static void applyPath(vtkSmartPointer<vtkPolyData> poly_data, vtkSmartPointer<vtkAppendPolyData> append_filter, const std::vector<Affine3f> &path)
{
vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
mat_trans->Identity();
vtkIdType nr_points = path.size();
for (vtkIdType i = 0; i < nr_points; ++i)
{
vtkSmartPointer<vtkPolyData> new_data = vtkSmartPointer<vtkPolyData>::New();
new_data->DeepCopy(poly_data);
// Transform the default coordinate frame
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
transform->PreMultiply();
vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans);
transform->SetMatrix(mat_trans);
vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
filter->SetInput(new_data);
filter->SetTransform(transform);
filter->Update();
append_filter->AddInputConnection(filter->GetOutputPort());
}
}
};
cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Color &color, bool show_frames, double scale)
{
vtkIdType nr_points = path.size();
......@@ -1016,49 +1046,33 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, c
vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
if (show_frames)
{
vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
mat_trans->Identity();
vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New();
axes->SetOrigin (0, 0, 0);
axes->SetScaleFactor (scale);
for (vtkIdType i = 0; i < nr_points; ++i)
{
vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New();
axes->SetOrigin (0, 0, 0);
axes->SetScaleFactor (scale);
vtkSmartPointer<vtkUnsignedCharArray> axes_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
axes_colors->SetNumberOfComponents(3);
axes_colors->InsertNextTuple3(255,0,0);
axes_colors->InsertNextTuple3(255,0,0);
axes_colors->InsertNextTuple3(0,255,0);
axes_colors->InsertNextTuple3(0,255,0);
axes_colors->InsertNextTuple3(0,0,255);
axes_colors->InsertNextTuple3(0,0,255);
vtkSmartPointer<vtkPolyData> axes_data = axes->GetOutput ();
axes_data->Update ();
axes_data->GetPointData ()->SetScalars (axes_colors);
// Transform the default coordinate frame
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
transform->PreMultiply();
vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans);
transform->SetMatrix(mat_trans);
vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
filter->SetInput(axes_data);
filter->SetTransform(transform);
filter->Update();
vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
axes_tubes->SetInput (filter->GetOutput());
axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0);
axes_tubes->SetNumberOfSides (6);
appendFilter->AddInputConnection(axes_tubes->GetOutputPort());
}
vtkSmartPointer<vtkUnsignedCharArray> axes_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
axes_colors->SetNumberOfComponents(3);
axes_colors->InsertNextTuple3(255,0,0);
axes_colors->InsertNextTuple3(255,0,0);
axes_colors->InsertNextTuple3(0,255,0);
axes_colors->InsertNextTuple3(0,255,0);
axes_colors->InsertNextTuple3(0,0,255);
axes_colors->InsertNextTuple3(0,0,255);
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);
axes_tubes->Update();
ApplyPath::applyPath(axes_tubes->GetOutput(), appendFilter, path);
}
// Set the color only for polyData
// Set the color for polyData
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
colors->SetNumberOfComponents(3);
......@@ -1103,36 +1117,19 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, c
double planesArray[24];
camera->GetFrustumPlanes(aspect_ratio, planesArray);
vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
mat_trans->Identity();
vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
planes->SetFrustumPlanes(planesArray);
for (vtkIdType i = 0; i < nr_points; ++i)
{
vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
planes->SetFrustumPlanes(planesArray);
vtkSmartPointer<vtkFrustumSource> frustumSource = vtkSmartPointer<vtkFrustumSource>::New();
frustumSource->SetPlanes(planes);
frustumSource->Update();
// Extract the edges
vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
filter->SetInput(frustumSource->GetOutput());
filter->Update();
// Transform the default coordinate frame
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
transform->PreMultiply();
vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans);
transform->SetMatrix(mat_trans);
vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
transform_filter->SetInput(filter->GetOutput());
transform_filter->SetTransform(transform);
transform_filter->Update();
appendFilter->AddInputConnection(transform_filter->GetOutputPort());
}
vtkSmartPointer<vtkFrustumSource> frustumSource = vtkSmartPointer<vtkFrustumSource>::New();
frustumSource->SetPlanes(planes);
frustumSource->Update();
// Extract the edges
vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
filter->SetInput(frustumSource->GetOutput());
filter->Update();
ApplyPath::applyPath(filter->GetOutput(), appendFilter, path);
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetInput(appendFilter->GetOutput());
......
modules/viz/test/test_viz3d.cpp
View file @ c0f44ca4
......@@ -89,7 +89,9 @@ TEST(Viz_viz3d, accuracy)
viz::Color color = viz::Color::black();
viz::LineWidget lw(Point3f(0, 0, 0), Point3f(4.f, 4.f,4.f), viz::Color::green());
viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0), 5.0);
viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0));
viz::PlaneWidget pw2(Vec4f(0.0,1.0,2.0,3.0), 2.0, viz::Color::red());
viz::PlaneWidget pw3(Vec4f(0.0,1.0,2.0,3.0), 3.0, viz::Color::blue());
viz::SphereWidget sw(Point3f(0, 0, 0), 0.2);
viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), 0.01, viz::Color::red());
viz::CircleWidget cw(Point3f(0, 0, 0), 0.5, 0.01, viz::Color::green());
......@@ -101,8 +103,10 @@ TEST(Viz_viz3d, accuracy)
viz::CloudWidget pcw2(cloud, viz::Color::magenta());
// viz.showWidget("line", lw);
// viz.showWidget("plane", pw);
viz.showWidget("sphere", sw);
viz.showWidget("plane", pw);
viz.showWidget("plane2", pw2);
viz.showWidget("plane3", pw3);
// viz.showWidget("sphere", sw);
// viz.showWidget("arrow", aw);
// viz.showWidget("circle", cw);
// viz.showWidget("cylinder", cyw);
......@@ -145,16 +149,33 @@ TEST(Viz_viz3d, accuracy)
viz::CameraPositionWidget cpw(Vec3f(0.5, 0.5, 3.0), Vec3f(0.0,0.0,0.0), Vec3f(0.0,-1.0,0.0), 0.5);
viz::CameraPositionWidget cpw2(0.5);
viz::CameraPositionWidget frustum(K, 2.0, viz::Color::green());
// viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
viz::CameraPositionWidget frustum3(Vec2f(CV_PI, CV_PI/2), 4.0);
viz::Text3DWidget t3w1("Camera1", Point3f(0.4, 0.6, 3.0), 0.1);
viz::Text3DWidget t3w2("Camera2", Point3f(0,0,0), 0.1);
viz.showWidget("CameraPositionWidget", cpw);
viz.showWidget("CameraPositionWidget2", cpw2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
viz.showWidget("camera_label", t3w1);
viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
// viz.showWidget("CameraPositionWidget", cpw);
// viz.showWidget("CameraPositionWidget2", cpw2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
// viz.showWidget("camera_label", t3w1);
// viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
// viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
// viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
// viz.showWidget("frustum3", frustum3, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
std::vector<Affine3f> trajectory;
trajectory.push_back(Affine3f().translate(Vec3f(0.5,0.5,0.5)));
trajectory.push_back(Affine3f().translate(Vec3f(1.0,0.0,0.0)));
trajectory.push_back(Affine3f().translate(Vec3f(2.0,0.5,0.0)));
trajectory.push_back(Affine3f(0.5, 0.0, 0.0, Vec3f(1.0,0.0,1.0)));
//
viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory, viz::Color(0,255,255), true, 0.5));
viz.showWidget("trajectory2", viz::TrajectoryWidget(trajectory, K, 1.0, viz::Color(255,0,255)));
// viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory/*, viz::Color::yellow()*/));
// viz.showWidget("CameraPositionWidget2", cpw2);
// viz.showWidget("CameraPositionWidget3", cpw3);
......
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