ported some trajectories functionality to InputArray style
This commit is contained in:
Anatoly Baksheev
parent
7b28f730f4
commit
e26b7e1e4f
@ -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.
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
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;
|
||||
|
||||
// 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();
|
||||
if (_path.depth() == CV_32F)
|
||||
path.assign(fpath, fend);
|
||||
|
||||
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
|
||||
points->SetDataTypeToFloat();
|
||||
points->SetNumberOfPoints(nr_points);
|
||||
if (_path.depth() == CV_64F)
|
||||
path.assign(dpath, dend);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
for(int i = start; i < end; ++i)
|
||||
Mat(traj).convertTo(_traj, _traj.depth());
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}}}
|
||||
|
||||
CV_Assert(!"Unsupported array kind");
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// 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());
|
||||
|
||||
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); }
|
||||
|
||||
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); }
|
||||
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();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user