Skip to content

O
opencv
Commit e26b7e1e authored by Anatoly Baksheev's avatar Anatoly Baksheev
Browse files
Options

ported some trajectories functionality to InputArray style

parent 7b28f730
Hide whitespace changes
Inline Side-by-side
Showing with 120 additions and 140 deletions
modules/viz/doc/widget.rst
View file @ e26b7e1e
......@@ -732,16 +732,16 @@ This 3D Widget represents a trajectory. ::
enum {FRAMES = 1, PATH = 2, BOTH = FRAMES + PATH};
//! Displays trajectory of the given path either by coordinate frames or polyline
WTrajectory(const std::vector<Affine3d> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white(),;
WTrajectory(InputArray path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white(),;
};
viz::WTrajectory::WTrajectory
-----------------------------
Constructs a WTrajectory.
.. ocv:function:: WTrajectory(const std::vector<Affine3d> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white())
.. ocv:function:: WTrajectory(InputArray path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white())
:param path: List of poses on a trajectory.
:param path: List of poses on a trajectory. Takes std::vector<Affine<T>> with T == [float | double]
:param display_mode: Display mode. This can be PATH, FRAMES, and BOTH.
:param scale: Scale of the frames. Polyline is not affected.
:param color: :ocv:class:`Color` of the polyline that represents path. Frames are not affected.
......
modules/viz/include/opencv2/viz.hpp
View file @ e26b7e1e
......@@ -104,11 +104,17 @@ namespace cv
CV_EXPORTS bool readPose(const String& file, Affine3d& pose, const String& tag = "pose");
CV_EXPORTS void writePose(const String& file, const Affine3d& pose, const String& tag = "pose");
CV_EXPORTS void writeTrajectory(const std::vector<Affine3f>& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
CV_EXPORTS void writeTrajectory(const std::vector<Affine3d>& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
//! takes vector<Affine3<T>> with T = float/dobule and writes to a sequence of files with given filename format
CV_EXPORTS void writeTrajectory(InputArray traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
CV_EXPORTS void readTrajectory(std::vector<Affine3f>& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
CV_EXPORTS void readTrajectory(std::vector<Affine3d>& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
//! takes vector<Affine3<T>> with T = float/dobule and loads poses from sequence of files
CV_EXPORTS void readTrajectory(OutputArray traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
///////////////////////////////////////////////////////////////////////////////////////////////
/// Computing normals for mesh
CV_EXPORTS void computeNormals(const Mesh3d& mesh, OutputArray normals);
} /* namespace viz */
} /* namespace cv */
......
modules/viz/include/opencv2/viz/widgets.hpp
View file @ e26b7e1e
......@@ -262,9 +262,8 @@ namespace cv
public:
enum {FRAMES = 1, PATH = 2, BOTH = FRAMES + PATH };
//! Displays trajectory of the given path either by coordinate frames or polyline
WTrajectory(const std::vector<Affine3f> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white());
WTrajectory(const std::vector<Affine3d> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white());
//! Takes vector<Affine3<T>> and displays trajectory of the given path either by coordinate frames or polyline
WTrajectory(InputArray path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white());
};
class CV_EXPORTS WTrajectoryFrustums : public Widget3D
......@@ -309,7 +308,7 @@ namespace cv
class CV_EXPORTS WCloudNormals : public Widget3D
{
public:
WCloudNormals(InputArray cloud, InputArray normals, int level = 100, float scale = 0.02f, const Color &color = Color::white());
WCloudNormals(InputArray cloud, InputArray normals, int level = 64, float scale = 0.1f, const Color &color = Color::white());
};
class CV_EXPORTS WMesh : public Widget3D
......
modules/viz/src/shapes.cpp
View file @ e26b7e1e
......@@ -1213,106 +1213,21 @@ namespace cv { namespace viz { namespace
};
}}}
cv::viz::WTrajectory::WTrajectory(const std::vector<Affine3f> &_path, int display_mode, float scale, const Color &color)
cv::viz::WTrajectory::WTrajectory(InputArray _path, int display_mode, float scale, const Color &color)
{
std::vector<Affine3d> path(_path.begin(), _path.end());
CV_Assert(_path.kind() == _InputArray::STD_VECTOR || _path.kind() == _InputArray::MAT);
CV_Assert(_path.type() == CV_32FC(16) || _path.type() == CV_64FC(16));
vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
// Bitwise and with 3 in order to limit the domain to 2 bits
if ((~display_mode & 3) ^ WTrajectory::PATH)
{
// Create a poly line along the path
vtkIdType nr_points = path.size();
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
points->SetDataTypeToFloat();
points->SetNumberOfPoints(nr_points);
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
polyLine->GetPointIds()->SetNumberOfIds(nr_points);
Vec3f *data_beg = vtkpoints_data<float>(points);
for (vtkIdType i = 0; i < nr_points; ++i)
{
Vec3f cam_pose = path[i].translation();
*data_beg++ = cam_pose;
polyLine->GetPointIds()->SetId(i,i);
}
vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
cells->InsertNextCell(polyLine);
polyData->SetPoints(points);
polyData->SetLines(cells);
// Set the color for polyData
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
colors->SetNumberOfComponents(3);
colors->SetNumberOfTuples(nr_points);
colors->FillComponent(0, color[2]);
colors->FillComponent(1, color[1]);
colors->FillComponent(2, color[0]);
polyData->GetPointData()->SetScalars(colors);
#if VTK_MAJOR_VERSION <= 5
appendFilter->AddInputConnection(polyData->GetProducerPort());
#else
appendFilter->AddInputData(polyData);
#endif
}
const Affine3d* dpath = _path.getMat().ptr<Affine3d>(), *dend = dpath + _path.total();
const Affine3f* fpath = _path.getMat().ptr<Affine3f>(), *fend = fpath + _path.total();
std::vector<Affine3d> path;
if ((~display_mode & 3) ^ WTrajectory::FRAMES)
{
// Create frames and transform along the path
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();
#if VTK_MAJOR_VERSION <= 5
axes_data->Update();
#else
axes->Update();
#endif
axes_data->GetPointData()->SetScalars(axes_colors);
if (_path.depth() == CV_32F)
path.assign(fpath, fend);
vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New();
#if VTK_MAJOR_VERSION <= 5
axes_tubes->SetInput(axes_data);
#else
axes_tubes->SetInputData(axes_data);
#endif
axes_tubes->SetRadius(axes->GetScaleFactor() / 50.0);
axes_tubes->SetNumberOfSides(6);
axes_tubes->Update();
TrajectoryUtils::applyPath(axes_tubes->GetOutput(), appendFilter, path);
}
if (_path.depth() == CV_64F)
path.assign(dpath, dend);
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetScalarModeToUsePointData();
mapper->SetInputConnection(appendFilter->GetOutputPort());
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);
WidgetAccessor::setProp(*this, actor);
}
cv::viz::WTrajectory::WTrajectory(const std::vector<Affine3d> &path, int display_mode, float scale, const Color &color)
{
vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
// Bitwise and with 3 in order to limit the domain to 2 bits
......
modules/viz/src/vizcore.cpp
View file @ e26b7e1e
......@@ -243,53 +243,103 @@ void cv::viz::writePose(const String& file, const Affine3d& pose, const String&
fs << tag << Mat(pose.matrix, false);
}
namespace cv { namespace viz { namespace impl
void cv::viz::readTrajectory(OutputArray _traj, const String& files_format, int start, int end, const String& tag)
{
template <typename _Tp>
void readTrajectory(std::vector<Affine3<_Tp> >& traj, const String& files_format, int start, int end, const String& tag)
CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT);
start = max(0, std::min(start, end));
end = std::max(start, end);
std::vector<Affine3d> traj;
for(int i = start; i < end; ++i)
{
start = max(0, std::min(start, end));
end = std::max(start, end);
Affine3d affine;
bool ok = readPose(cv::format(files_format.c_str(), i), affine, tag);
if (!ok)
break;
std::vector< Affine3<_Tp> > temp;
traj.push_back(affine);
}
Mat(traj).convertTo(_traj, _traj.depth());
}
for(int i = start; i < end; ++i)
void cv::viz::writeTrajectory(InputArray _traj, const String& files_format, int start, const String& tag)
{
if (_traj.kind() == _InputArray::STD_VECTOR_MAT)
{
std::vector<Mat>& v = *(std::vector<Mat>*)_traj.getObj();
for(size_t i = 0, index = max(0, start); i < v.size(); ++i, ++index)
{
Affine3d affine;
bool ok = readPose(cv::format(files_format.c_str(), i), affine, tag);
if (!ok)
break;
temp.push_back(affine);
Mat pose = v[i];
CV_Assert(pose.type() == CV_32FC(16) || pose.type() == CV_64FC(16));
pose.copyTo(affine.matrix);
writePose(cv::format(files_format.c_str(), index), affine, tag);
}
traj.swap(temp);
return;
}
template <typename _Tp>
void writeTrajectory(const std::vector<Affine3<_Tp> >& traj, const String& files_format, int start, const String& tag)
if (_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT)
{
for(size_t i = 0, index = max(0, start); i < traj.size(); ++i, ++index)
writePose(cv::format(files_format.c_str(), index), traj[i], tag);
}
}}}
CV_Assert(_traj.type() == CV_32FC(16) || _traj.type() == CV_64FC(16));
Mat traj = _traj.getMat();
if (traj.depth() == CV_32F)
for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index)
writePose(cv::format(files_format.c_str(), index), traj.at<Affine3f>(i), tag);
if (traj.depth() == CV_64F)
for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index)
writePose(cv::format(files_format.c_str(), index), traj.at<Affine3d>(i), tag);
}
void cv::viz::readTrajectory(std::vector<Affine3f>& traj, const String& files_format, int start, int end, const String& tag)
{ impl::readTrajectory(traj, files_format, start, end, tag); }
void cv::viz::readTrajectory(std::vector<Affine3d>& traj, const String& files_format, int start, int end, const String& tag)
{ impl::readTrajectory(traj, files_format, start, end, tag); }
void cv::viz::writeTrajectory(const std::vector<Affine3f>& traj, const String& files_format, int start, const String& tag)
{ impl::writeTrajectory(traj, files_format, start, tag); }
CV_Assert(!"Unsupported array kind");
}
void cv::viz::writeTrajectory(const std::vector<Affine3d>& traj, const String& files_format, int start, const String& tag)
{ impl::writeTrajectory(traj, files_format, start, tag); }
///////////////////////////////////////////////////////////////////////////////////////////////
/// Computing normals for mesh
void cv::viz::computeNormals(const Mesh3d& mesh, OutputArray _normals)
{
vtkSmartPointer<vtkProp> prop = WidgetAccessor::getProp(WMesh(mesh));
vtkSmartPointer<vtkMapper> mapper = vtkActor::SafeDownCast(prop)->GetMapper();
vtkSmartPointer<vtkPolyData> polydata = vtkPolyData::SafeDownCast(mapper->GetInput());
vtkSmartPointer<vtkPolyDataNormals> normal_generator = vtkSmartPointer<vtkPolyDataNormals>::New();
#if VTK_MAJOR_VERSION <= 5
normal_generator->SetInput(polydata);
#else
normal_generator->SetInputData(polydata);
#endif
normal_generator->ComputePointNormalsOn();
normal_generator->ComputeCellNormalsOff();
normal_generator->SetFeatureAngle(0.1);
normal_generator->SetSplitting(0);
normal_generator->SetConsistency(1);
normal_generator->SetAutoOrientNormals(0);
normal_generator->SetFlipNormals(0);
normal_generator->SetNonManifoldTraversal(1);
normal_generator->Update();
vtkSmartPointer<vtkDataArray> generic_normals = normal_generator->GetOutput()->GetPointData()->GetNormals();
if(generic_normals)
{
Mat normals(1, generic_normals->GetNumberOfTuples(), CV_64FC3);
Vec3d *optr = normals.ptr<Vec3d>();
for(int i = 0; i < generic_normals->GetNumberOfTuples(); ++i, ++optr)
generic_normals->GetTuple(i, optr->val);
normals.convertTo(_normals, mesh.cloud.type());
}
else
_normals.release();
}
......
modules/viz/test/tests_simple.cpp
View file @ e26b7e1e
......@@ -49,9 +49,11 @@ TEST(Viz, DISABLED_show_cloud_bluberry)
{
Mat dragon_cloud = readCloud(get_dragon_ply_file_path());
Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
Viz3d viz("show_cloud_bluberry");
viz.showWidget("coosys", WCoordinateSystem());
viz.showWidget("dragon", WCloud(dragon_cloud, Color::bluberry()));
viz.showWidget("dragon", WCloud(dragon_cloud, Color::bluberry()), pose);
viz.spin();
}
......@@ -62,9 +64,11 @@ TEST(Viz, DISABLED_show_cloud_random_color)
Mat colors(dragon_cloud.size(), CV_8UC3);
theRNG().fill(colors, RNG::UNIFORM, 0, 255);
Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
Viz3d viz("show_cloud_random_color");
viz.showWidget("coosys", WCoordinateSystem());
viz.showWidget("dragon", WCloud(dragon_cloud, colors));
viz.showWidget("dragon", WCloud(dragon_cloud, colors), pose);
viz.spin();
}
......@@ -77,9 +81,11 @@ TEST(Viz, DISABLED_show_cloud_masked)
if (i % 15 != 0)
dragon_cloud.at<Vec3f>(i) = qnan;
Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
Viz3d viz("show_cloud_masked");
viz.showWidget("coosys", WCoordinateSystem());
viz.showWidget("dragon", WCloud(dragon_cloud));
viz.showWidget("dragon", WCloud(dragon_cloud), pose);
viz.spin();
}
......@@ -102,9 +108,11 @@ TEST(Viz, DISABLED_show_mesh)
{
Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());
Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
Viz3d viz("show_mesh");
viz.showWidget("coosys", WCoordinateSystem());
viz.showWidget("mesh", WMesh(mesh));
viz.showWidget("mesh", WMesh(mesh), pose);
viz.spin();
}
......@@ -113,9 +121,11 @@ TEST(Viz, DISABLED_show_mesh_random_colors)
Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());
theRNG().fill(mesh.colors, RNG::UNIFORM, 0, 255);
Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
Viz3d viz("show_mesh_random_color");
viz.showWidget("coosys", WCoordinateSystem());
viz.showWidget("mesh", WMesh(mesh));
viz.showWidget("mesh", WMesh(mesh), pose);
viz.setRenderingProperty("mesh", SHADING, SHADING_PHONG);
viz.spin();
}
......
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