void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
-void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, InputArray _colors, const Affine3f& pose)
-{
- Mat cloud = _cloud.getMat();
- Mat colors = _colors.getMat();
- CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
- CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size());
-
- vtkSmartPointer<vtkPolyData> polydata;
- vtkSmartPointer<vtkCellArray> vertices;
- vtkSmartPointer<vtkPoints> points;
- vtkSmartPointer<vtkIdTypeArray> initcells;
- vtkIdType nr_points = cloud.total();
-
- // If the cloud already exists, update otherwise create new one
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
- bool exist = am_it == cloud_actor_map_->end();
- if (exist)
- {
- // Add as new cloud
- allocVtkPolyData(polydata);
- //polydata = vtkSmartPointer<vtkPolyData>::New ();
- vertices = vtkSmartPointer<vtkCellArray>::New ();
- polydata->SetVerts (vertices);
-
- points = polydata->GetPoints ();
-
- if (!points)
- {
- points = vtkSmartPointer<vtkPoints>::New ();
- if (cloud.depth() == CV_32F)
- points->SetDataTypeToFloat();
- else if (cloud.depth() == CV_64F)
- points->SetDataTypeToDouble();
- polydata->SetPoints (points);
- }
- points->SetNumberOfPoints (nr_points);
- }
- else
- {
- // Update the cloud
- // Get the current poly data
- polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
- vertices = polydata->GetVerts ();
- points = polydata->GetPoints ();
- // Update the point data type based on the cloud
- if (cloud.depth() == CV_32F)
- points->SetDataTypeToFloat ();
- else if (cloud.depth() == CV_64F)
- points->SetDataTypeToDouble ();
-
- points->SetNumberOfPoints (nr_points);
- }
-
- if (cloud.depth() == CV_32F)
- {
- // Get a pointer to the beginning of the data array
- Vec3f *data_beg = vtkpoints_data<float>(points);
- Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud);
- std::transform(data_beg, data_end, data_beg, ApplyAffine(pose));
- nr_points = data_end - data_beg;
-
- }
- else if (cloud.depth() == CV_64F)
- {
- // Get a pointer to the beginning of the data array
- Vec3d *data_beg = vtkpoints_data<double>(points);
- Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud);
- std::transform(data_beg, data_end, data_beg, ApplyAffine(pose));
- nr_points = data_end - data_beg;
- }
-
- points->SetNumberOfPoints (nr_points);
-
- vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
-
- if (exist)
- updateCells (cells, initcells, nr_points);
- else
- updateCells (cells, am_it->second.cells, nr_points);
-
- // Set the cells and the vertices
- vertices->SetCells (nr_points, cells);
-
- // Get a random color
- Vec3b* colors_data = new Vec3b[nr_points];
- NanFilter::copy(colors, colors_data, cloud);
-
- vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
- scalars->SetNumberOfComponents (3);
- scalars->SetNumberOfTuples (nr_points);
- scalars->SetArray (colors_data->val, 3 * nr_points, 0);
-
- // Assign the colors
- Vec2d minmax;
- polydata->GetPointData ()->SetScalars (scalars);
- scalars->GetRange (minmax.val);
-
- // If this is the new point cloud, a new actor is created
- if (exist)
- {
- vtkSmartPointer<vtkLODActor> actor;
- createActorFromVTKDataSet (polydata, actor);
-
- actor->GetMapper ()->SetScalarRange (minmax.val);
-
- // Add it to all renderers
- renderer_->AddActor (actor);
-
- // Save the pointer/ID pair to the global actor map
- (*cloud_actor_map_)[id].actor = actor;
- (*cloud_actor_map_)[id].cells = initcells;
-
- const Eigen::Vector4f sensor_origin = Eigen::Vector4f::Zero ();
- const Eigen::Quaternionf sensor_orientation = Eigen::Quaternionf::Identity ();
-
- // Save the viewpoint transformation matrix to the global actor map
- vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
- convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
-
- (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
- }
- else
- {
- // Update the mapper
- reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
- am_it->second.actor->GetMapper ()->ScalarVisibilityOn();
- am_it->second.actor->Modified ();
- }
-}
-
-void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, const Color& color, const Affine3f& pose)
-{
- Mat cloud = _cloud.getMat();
- CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
-
- vtkSmartPointer<vtkPolyData> polydata;
- vtkSmartPointer<vtkCellArray> vertices;
- vtkSmartPointer<vtkPoints> points;
- vtkSmartPointer<vtkIdTypeArray> initcells;
- vtkIdType nr_points = cloud.total();
-
- // If the cloud already exists, update otherwise create new one
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
- bool exist = am_it == cloud_actor_map_->end();
- if (exist)
- {
- // Add as new cloud
- allocVtkPolyData(polydata);
- //polydata = vtkSmartPointer<vtkPolyData>::New ();
- vertices = vtkSmartPointer<vtkCellArray>::New ();
- polydata->SetVerts (vertices);
-
- points = polydata->GetPoints ();
-
- if (!points)
- {
- points = vtkSmartPointer<vtkPoints>::New ();
- if (cloud.depth() == CV_32F)
- points->SetDataTypeToFloat();
- else if (cloud.depth() == CV_64F)
- points->SetDataTypeToDouble();
- polydata->SetPoints (points);
- }
- points->SetNumberOfPoints (nr_points);
- }
- else
- {
- // Update the cloud
- // Get the current poly data
- polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
- vertices = polydata->GetVerts ();
- points = polydata->GetPoints ();
- // Update the point data type based on the cloud
- if (cloud.depth() == CV_32F)
- points->SetDataTypeToFloat ();
- else if (cloud.depth() == CV_64F)
- points->SetDataTypeToDouble ();
-
- points->SetNumberOfPoints (nr_points);
- }
-
- if (cloud.depth() == CV_32F)
- {
- // Get a pointer to the beginning of the data array
- Vec3f *data_beg = vtkpoints_data<float>(points);
- Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud);
- std::transform(data_beg, data_end, data_beg, ApplyAffine(pose));
- nr_points = data_end - data_beg;
-
- }
- else if (cloud.depth() == CV_64F)
- {
- // Get a pointer to the beginning of the data array
- Vec3d *data_beg = vtkpoints_data<double>(points);
- Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud);
- std::transform(data_beg, data_end, data_beg, ApplyAffine(pose));
- nr_points = data_end - data_beg;
- }
-
- points->SetNumberOfPoints (nr_points);
-
- vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
-
- if (exist)
- updateCells (cells, initcells, nr_points);
- else
- updateCells (cells, am_it->second.cells, nr_points);
-
- // Set the cells and the vertices
- vertices->SetCells (nr_points, cells);
-
- // Get a random color
- Color c = vtkcolor(color);
- polydata->GetPointData ()->SetScalars (0);
-
- // If this is the new point cloud, a new actor is created
- if (exist)
- {
- vtkSmartPointer<vtkLODActor> actor;
- createActorFromVTKDataSet (polydata, actor, false);
-
- actor->GetProperty ()->SetColor(c.val);
-
- // Add it to all renderers
- renderer_->AddActor (actor);
-
- // Save the pointer/ID pair to the global actor map
- (*cloud_actor_map_)[id].actor = actor;
- (*cloud_actor_map_)[id].cells = initcells;
-
- const Eigen::Vector4f sensor_origin = Eigen::Vector4f::Zero ();
- const Eigen::Quaternionf sensor_orientation = Eigen::Quaternionf::Identity ();
-
- // Save the viewpoint transformation matrix to the global actor map
- vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
- convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
-
- (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
- }
- else
- {
- // Update the mapper
- reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
- am_it->second.actor->GetProperty ()->SetColor(c.val);
- am_it->second.actor->GetMapper ()->ScalarVisibilityOff();
- am_it->second.actor->Modified ();
- }
-}
-
-bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level, float scale, const std::string &id)
-{
- CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3);
-
- if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
- return false;
-
- vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
- vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
-
- points->SetDataTypeToFloat ();
- vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
- data->SetNumberOfComponents (3);
-
- vtkIdType nr_normals = 0;
- float* pts = 0;
-
- // If the cloud is organized, then distribute the normal step in both directions
- if (cloud.cols > 1 && cloud.rows > 1)
- {
- vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
- nr_normals = (static_cast<vtkIdType> ((cloud.cols - 1)/ point_step) + 1) *
- (static_cast<vtkIdType> ((cloud.rows - 1) / point_step) + 1);
- pts = new float[2 * nr_normals * 3];
-
- vtkIdType cell_count = 0;
- for (vtkIdType y = 0; y < cloud.rows; y += point_step)
- for (vtkIdType x = 0; x < cloud.cols; x += point_step)
- {
- cv::Point3f p = cloud.at<cv::Point3f>(y, x);
- cv::Point3f n = normals.at<cv::Point3f>(y, x) * scale;
-
- pts[2 * cell_count * 3 + 0] = p.x;
- pts[2 * cell_count * 3 + 1] = p.y;
- pts[2 * cell_count * 3 + 2] = p.z;
- pts[2 * cell_count * 3 + 3] = p.x + n.x;
- pts[2 * cell_count * 3 + 4] = p.y + n.y;
- pts[2 * cell_count * 3 + 5] = p.z + n.z;
-
- lines->InsertNextCell (2);
- lines->InsertCellPoint (2 * cell_count);
- lines->InsertCellPoint (2 * cell_count + 1);
- cell_count++;
- }
- }
- else
- {
- nr_normals = (cloud.size().area() - 1) / level + 1 ;
- pts = new float[2 * nr_normals * 3];
-
- for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
- {
- cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
- cv::Point3f n = normals.ptr<cv::Point3f>()[i] * scale;
-
- pts[2 * j * 3 + 0] = p.x;
- pts[2 * j * 3 + 1] = p.y;
- pts[2 * j * 3 + 2] = p.z;
- pts[2 * j * 3 + 3] = p.x + n.x;
- pts[2 * j * 3 + 4] = p.y + n.y;
- pts[2 * j * 3 + 5] = p.z + n.z;
-
- lines->InsertNextCell (2);
- lines->InsertCellPoint (2 * j);
- lines->InsertCellPoint (2 * j + 1);
- }
- }
-
- data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
- points->SetData (data);
-
- vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
- polyData->SetPoints (points);
- polyData->SetLines (lines);
-
- vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
- mapper->SetInput (polyData);
- mapper->SetColorModeToMapScalars();
- mapper->SetScalarModeToUsePointData();
-
- // create actor
- vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
- actor->SetMapper (mapper);
-
- // Add it to all renderers
- renderer_->AddActor (actor);
-
- // Save the pointer/ID pair to the global actor map
- (*cloud_actor_map_)[id].actor = actor;
- 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 ());
return (true);
}
-//////////////////////////////////////////////////
-bool temp_viz::Viz3d::VizImpl::addText3D (const std::string &text, const cv::Point3f& position, const Color& color, double textScale, const std::string &id)
-{
- std::string tid;
- if (id.empty ())
- tid = text;
- else
- tid = id;
-
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
- if (am_it != shape_actor_map_->end ())
- return std::cout << "[addText3d] A text with id <" << tid << "> already exists! Please choose a different id and retry." << std::endl, false;
-
- vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
- textSource->SetText (text.c_str());
- textSource->Update ();
-
- vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
- textMapper->SetInputConnection (textSource->GetOutputPort ());
-
- // Since each follower may follow a different camera, we need different followers
- vtkRenderer* renderer = renderer_;
-
- vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New ();
- textActor->SetMapper (textMapper);
- textActor->SetPosition (position.x, position.y, position.z);
- textActor->SetScale (textScale);
-
- Color c = vtkcolor(color);
- textActor->GetProperty ()->SetColor (c.val);
- textActor->SetCamera (renderer->GetActiveCamera ());
-
- renderer->AddActor (textActor);
- renderer->Render ();
-
- // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
- // for multiple viewport
- (*shape_actor_map_)[tid] = textActor;
-
-
- return (true);
-}
-
bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& color, const std::string &id)
{
CV_Assert(cloud.type() == CV_32FC3 && cloud.rows == 1);
actor->SetUserMatrix (matrix);
actor->Modified();
}
+ // If the actor is a vtkFollower, then it should always face the camera
+ vtkFollower *follower = vtkFollower::SafeDownCast(actor);
+ if (follower)
+ {
+ follower->SetCamera(renderer_->GetActiveCamera());
+ }
+
renderer_->AddActor(WidgetAccessor::getProp(widget));
(*widget_actor_map_)[id].actor = WidgetAccessor::getProp(widget);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
-void temp_viz::Viz3d::VizImpl::setPointCloudColor (const Color& color, const std::string &id)
-{
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
- if (am_it != cloud_actor_map_->end ())
- {
- vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
-
- Color c = vtkcolor(color);
- actor->GetProperty ()->SetColor (c.val);
- actor->GetMapper ()->ScalarVisibilityOff ();
- actor->Modified ();
- }
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::getPointCloudRenderingProperties (int property, double &value, const std::string &id)
{
CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
}
/////////////////////////////////////////////////////////////////////////////////////////////
-void temp_viz::Viz3d::VizImpl::setShapeColor (const Color& color, const std::string &id)
-{
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
- {
- vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
-
- Color c = vtkcolor(color);
- actor->GetMapper ()->ScalarVisibilityOff ();
- actor->GetProperty ()->SetColor (c.val);
- actor->GetProperty ()->SetEdgeColor (c.val);
- actor->GetProperty ()->SetAmbient (0.8);
- actor->GetProperty ()->SetDiffuse (0.8);
- actor->GetProperty ()->SetSpecular (0.8);
- actor->GetProperty ()->SetLighting (0);
- actor->Modified ();
- }
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::setShapeRenderingProperties (int property, double value, const std::string &id)
{
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
}
/////////////////////////////////////////////////////////////////////////////////////////////
-bool temp_viz::Viz3d::VizImpl::updateShapePose (const std::string &id, const cv::Affine3f& pose)
-{
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-
- vtkLODActor* actor;
-
- if (am_it == shape_actor_map_->end ())
- return (false);
- else
- actor = vtkLODActor::SafeDownCast (am_it->second);
-
- vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
-
- convertToVtkMatrix (pose.matrix, matrix);
-
- actor->SetUserMatrix (matrix);
- actor->Modified ();
-
- return (true);
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////////
void temp_viz::Viz3d::VizImpl::getCameras (temp_viz::Camera& camera)
{
vtkCamera* active_camera = renderer_->GetActiveCamera ();
return (true);
}
-/////////////////////////////////////////////////////////////////////////////////////////////
-bool temp_viz::Viz3d::VizImpl::addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
-{
- std::string tid = id.empty() ? text : id;
-
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
- if (am_it != shape_actor_map_->end ())
- return std::cout << "[addText] A text with id <"<<id <<"> already exists! Please choose a different id and retry.\n" << std::endl, false;
-
- // Create an Actor
- vtkSmartPointer<vtkTextActor> actor = vtkSmartPointer<vtkTextActor>::New ();
- actor->SetPosition (xpos, ypos);
- actor->SetInput (text.c_str ());
-
- vtkSmartPointer<vtkTextProperty> tprop = actor->GetTextProperty ();
- tprop->SetFontSize (fontsize);
- tprop->SetFontFamilyToArial ();
- tprop->SetJustificationToLeft ();
- tprop->BoldOn ();
-
- Color c = vtkcolor(color);
- tprop->SetColor (c.val);
- renderer_->AddActor(actor);
-
- // Save the pointer/ID pair to the global actor map
- (*shape_actor_map_)[tid] = actor;
- return (true);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////
-bool temp_viz::Viz3d::VizImpl::updateText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
-{
- std::string tid = id.empty() ? text : id;
-
- ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
- if (am_it == shape_actor_map_->end ())
- return false;
-
- // Retrieve the Actor
- vtkTextActor *actor = vtkTextActor::SafeDownCast (am_it->second);
-
- actor->SetPosition (xpos, ypos);
- actor->SetInput (text.c_str ());
-
- vtkTextProperty* tprop = actor->GetTextProperty ();
- tprop->SetFontSize (fontsize);
-
- Color c = vtkcolor(color);
- tprop->SetColor (c.val);
-
- actor->Modified ();
-
- return (true);
-}
-
bool temp_viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id)
{
CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);