generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

trajectory with spheres and lines (based on the code snippet in gdocs)

Browse Source
This commit is contained in:
ozantonkal 2013-07-26 14:50:11 +02:00
parent b661d72252
commit edfad34d39
2 changed files with 84 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -183,6 +183,8 @@ namespace cv
enum {DISPLAY_FRAMES = 1, DISPLAY_PATH = 2};
TrajectoryWidget(const std::vector<Affine3f> &path, int display_mode = TrajectoryWidget::DISPLAY_PATH, const Color &color = Color::white(), double scale = 1.0);
TrajectoryWidget(const std::vector<Affine3f> &path, float line_length, double init_sphere_radius,
double sphere_radius, const Color &line_color = Color::white(), const Color &sphere_color = Color::white());
TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
TrajectoryWidget(const std::vector<Affine3f> &path, const Vec2f &fov, double scale = 1.0, const Color &color = Color::white()); // Camera frustums

82
modules/viz/src/shape_widgets.cpp
View File

@ -1252,6 +1252,88 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, i
WidgetAccessor::setProp(*this, actor);
}
cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, float line_length, double init_sphere_radius,
double sphere_radius, const Color &line_color, const Color &sphere_color)
{
vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
vtkIdType nr_poses = path.size();
Point3f last_pos(0.0f,0.0f,0.0f);
// Create color arrays
vtkSmartPointer<vtkUnsignedCharArray> line_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
line_scalars->SetNumberOfComponents(3);
line_scalars->InsertNextTuple3(line_color[2], line_color[1], line_color[0]);
// Create color array for sphere
vtkSphereSource * dummy_sphere = vtkSphereSource::New();
// Create the array for big sphere
dummy_sphere->SetRadius(init_sphere_radius);
dummy_sphere->Update();
vtkIdType nr_points = dummy_sphere->GetOutput()->GetNumberOfCells();
vtkSmartPointer<vtkUnsignedCharArray> sphere_scalars_init = vtkSmartPointer<vtkUnsignedCharArray>::New();
sphere_scalars_init->SetNumberOfComponents(3);
sphere_scalars_init->SetNumberOfTuples(nr_points);
sphere_scalars_init->FillComponent(0, sphere_color[2]);
sphere_scalars_init->FillComponent(1, sphere_color[1]);
sphere_scalars_init->FillComponent(2, sphere_color[0]);
// Create the array for small sphere
dummy_sphere->SetRadius(sphere_radius);
dummy_sphere->Update();
nr_points = dummy_sphere->GetOutput()->GetNumberOfCells();
vtkSmartPointer<vtkUnsignedCharArray> sphere_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
sphere_scalars->SetNumberOfComponents(3);
sphere_scalars->SetNumberOfTuples(nr_points);
sphere_scalars->FillComponent(0, sphere_color[2]);
sphere_scalars->FillComponent(1, sphere_color[1]);
sphere_scalars->FillComponent(2, sphere_color[0]);
dummy_sphere->Delete();
for (vtkIdType i = 0; i < nr_poses; ++i)
{
Point3f new_pos = path[i] * last_pos;
vtkSmartPointer<vtkSphereSource> sphere_source = vtkSmartPointer<vtkSphereSource>::New();
sphere_source->SetCenter (new_pos.x, new_pos.y, new_pos.z);
if (i == 0)
{
sphere_source->SetRadius(init_sphere_radius);
sphere_source->Update();
sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars_init);
appendFilter->AddInputConnection(sphere_source->GetOutputPort());
last_pos = new_pos;
continue;
}
else
{
sphere_source->SetRadius(sphere_radius);
sphere_source->Update();
sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars);
appendFilter->AddInputConnection(sphere_source->GetOutputPort());
}
Vec3f v = last_pos - new_pos;
v = normalize(v) * line_length;
vtkSmartPointer<vtkLineSource> line_source = vtkSmartPointer<vtkLineSource>::New();
line_source->SetPoint1(new_pos.x + v[0], new_pos.y + v[1], new_pos.z + v[2]);
line_source->SetPoint2(new_pos.x, new_pos.y, new_pos.z);
line_source->Update();
line_source->GetOutput()->GetCellData()->SetScalars(line_scalars);
appendFilter->AddInputConnection(line_source->GetOutputPort());
last_pos = new_pos;
}
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetScalarModeToUseCellData();
mapper->SetInput(appendFilter->GetOutput());
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);
WidgetAccessor::setProp(*this, actor);
}
cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale, const Color &color)
{
vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();