added test for trajectories and fixed bug with float type
This commit is contained in:
parent
64566e6178
commit
25b417e8b2
@ -279,6 +279,7 @@ namespace cv
|
||||
class CV_EXPORTS WTrajectorySpheres: public Widget3D
|
||||
{
|
||||
public:
|
||||
//! Takes vector<Affine3<T>> and displays trajectory of the given path
|
||||
WTrajectorySpheres(InputArray path, double line_length = 0.05, double radius = 0.007,
|
||||
const Color &from = Color::red(), const Color &to = Color::white());
|
||||
};
|
||||
|
@ -221,6 +221,22 @@ namespace cv
|
||||
return scalars;
|
||||
}
|
||||
};
|
||||
|
||||
inline vtkSmartPointer<vtkMatrix4x4> vtkmatrix(const cv::Matx44d &matrix)
|
||||
{
|
||||
vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::New();
|
||||
vtk_matrix->DeepCopy(matrix.val);
|
||||
return vtk_matrix;
|
||||
}
|
||||
|
||||
inline Color vtkcolor(const Color& color)
|
||||
{
|
||||
Color scaled_color = color * (1.0/255.0);
|
||||
std::swap(scaled_color[0], scaled_color[2]);
|
||||
return scaled_color;
|
||||
}
|
||||
|
||||
template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/norm(v); }
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -70,6 +70,35 @@ template<> cv::viz::WLine cv::viz::Widget::cast<cv::viz::WLine>()
|
||||
return static_cast<WLine&>(widget);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// sphere widget implementation
|
||||
|
||||
cv::viz::WSphere::WSphere(const Point3d ¢er, double radius, int sphere_resolution, const Color &color)
|
||||
{
|
||||
vtkSmartPointer<vtkSphereSource> sphere = vtkSmartPointer<vtkSphereSource>::New();
|
||||
sphere->SetRadius(radius);
|
||||
sphere->SetCenter(center.x, center.y, center.z);
|
||||
sphere->SetPhiResolution(sphere_resolution);
|
||||
sphere->SetThetaResolution(sphere_resolution);
|
||||
sphere->LatLongTessellationOff();
|
||||
sphere->Update();
|
||||
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
|
||||
VtkUtils::SetInputData(mapper, sphere->GetOutput());
|
||||
|
||||
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
|
||||
actor->SetMapper(mapper);
|
||||
|
||||
WidgetAccessor::setProp(*this, actor);
|
||||
setColor(color);
|
||||
}
|
||||
|
||||
template<> cv::viz::WSphere cv::viz::Widget::cast<cv::viz::WSphere>()
|
||||
{
|
||||
Widget3D widget = this->cast<Widget3D>();
|
||||
return static_cast<WSphere&>(widget);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// plane widget implementation
|
||||
|
||||
@ -143,51 +172,21 @@ template<> cv::viz::WPlane cv::viz::Widget::cast<cv::viz::WPlane>()
|
||||
return static_cast<WPlane&>(widget);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// sphere widget implementation
|
||||
|
||||
cv::viz::WSphere::WSphere(const Point3d ¢er, double radius, int sphere_resolution, const Color &color)
|
||||
{
|
||||
vtkSmartPointer<vtkSphereSource> sphere = vtkSmartPointer<vtkSphereSource>::New();
|
||||
sphere->SetRadius(radius);
|
||||
sphere->SetCenter(center.x, center.y, center.z);
|
||||
sphere->SetPhiResolution(sphere_resolution);
|
||||
sphere->SetThetaResolution(sphere_resolution);
|
||||
sphere->LatLongTessellationOff();
|
||||
sphere->Update();
|
||||
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
|
||||
VtkUtils::SetInputData(mapper, sphere->GetOutput());
|
||||
|
||||
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
|
||||
actor->SetMapper(mapper);
|
||||
|
||||
WidgetAccessor::setProp(*this, actor);
|
||||
setColor(color);
|
||||
}
|
||||
|
||||
template<> cv::viz::WSphere cv::viz::Widget::cast<cv::viz::WSphere>()
|
||||
{
|
||||
Widget3D widget = this->cast<Widget3D>();
|
||||
return static_cast<WSphere&>(widget);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// arrow widget implementation
|
||||
|
||||
cv::viz::WArrow::WArrow(const Point3d& pt1, const Point3d& pt2, double thickness, const Color &color)
|
||||
{
|
||||
vtkSmartPointer<vtkArrowSource> arrowSource = vtkSmartPointer<vtkArrowSource>::New();
|
||||
arrowSource->SetShaftRadius(thickness);
|
||||
// The thickness and radius of the tip are adjusted based on the thickness of the arrow
|
||||
arrowSource->SetTipRadius(thickness * 3.0);
|
||||
arrowSource->SetTipLength(thickness * 10.0);
|
||||
vtkSmartPointer<vtkArrowSource> arrow_source = vtkSmartPointer<vtkArrowSource>::New();
|
||||
arrow_source->SetShaftRadius(thickness);
|
||||
arrow_source->SetTipRadius(thickness * 3.0);
|
||||
arrow_source->SetTipLength(thickness * 10.0);
|
||||
|
||||
RNG rng = theRNG();
|
||||
Vec3d arbitrary(rng.uniform(-10.0, 10.0), rng.uniform(-10.0, 10.0), rng.uniform(-10.0, 10.0));
|
||||
Vec3d startPoint(pt1.x, pt1.y, pt1.z), endPoint(pt2.x, pt2.y, pt2.z);
|
||||
|
||||
double length = cv::norm(endPoint - startPoint);
|
||||
double length = norm(endPoint - startPoint);
|
||||
|
||||
Vec3d xvec = normalized(endPoint - startPoint);
|
||||
Vec3d zvec = normalized(xvec.cross(arbitrary));
|
||||
@ -204,7 +203,7 @@ cv::viz::WArrow::WArrow(const Point3d& pt1, const Point3d& pt2, double thickness
|
||||
// Transform the polydata
|
||||
vtkSmartPointer<vtkTransformPolyDataFilter> transformPD = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
|
||||
transformPD->SetTransform(transform);
|
||||
transformPD->SetInputConnection(arrowSource->GetOutputPort());
|
||||
transformPD->SetInputConnection(arrow_source->GetOutputPort());
|
||||
transformPD->Update();
|
||||
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
|
||||
@ -908,14 +907,14 @@ namespace cv { namespace viz { namespace
|
||||
actor->SetTexture(texture);
|
||||
}
|
||||
|
||||
static vtkSmartPointer<vtkPolyData> createFrustrum(double aspect_ratio, double fovy, double scale)
|
||||
static vtkSmartPointer<vtkPolyData> createFrustum(double aspect_ratio, double fovy, double scale)
|
||||
{
|
||||
vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
|
||||
camera->SetViewAngle(fovy);
|
||||
camera->SetPosition(0.0, 0.0, 0.0);
|
||||
camera->SetViewUp(0.0, 1.0, 0.0);
|
||||
camera->SetFocalPoint(0.0, 0.0, 1.0);
|
||||
camera->SetClippingRange(0.01, scale);
|
||||
camera->SetClippingRange(1e-9, scale);
|
||||
|
||||
double planesArray[24];
|
||||
camera->GetFrustumPlanes(aspect_ratio, planesArray);
|
||||
@ -956,7 +955,7 @@ cv::viz::WCameraPosition::WCameraPosition(const Matx33d &K, double scale, const
|
||||
double fovy = 2.0 * atan2(c_y, f_y) * 180 / CV_PI;
|
||||
double aspect_ratio = f_y / f_x;
|
||||
|
||||
vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustrum(aspect_ratio, fovy, scale);
|
||||
vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustum(aspect_ratio, fovy, scale);
|
||||
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
|
||||
VtkUtils::SetInputData(mapper, polydata);
|
||||
@ -973,7 +972,7 @@ cv::viz::WCameraPosition::WCameraPosition(const Vec2d &fov, double scale, const
|
||||
double aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5);
|
||||
double fovy = fov[1] * 180 / CV_PI;
|
||||
|
||||
vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustrum(aspect_ratio, fovy, scale);
|
||||
vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustum(aspect_ratio, fovy, scale);
|
||||
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
|
||||
VtkUtils::SetInputData(mapper, polydata);
|
||||
|
@ -179,34 +179,16 @@ private:
|
||||
bool removeActorFromRenderer(const vtkSmartPointer<vtkProp> &actor);
|
||||
};
|
||||
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace viz
|
||||
{
|
||||
inline vtkSmartPointer<vtkMatrix4x4> vtkmatrix(const cv::Matx44d &matrix)
|
||||
{
|
||||
vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::New();
|
||||
vtk_matrix->DeepCopy(matrix.val);
|
||||
return vtk_matrix;
|
||||
}
|
||||
|
||||
struct color_tag {};
|
||||
struct gray_tag {};
|
||||
|
||||
inline Vec3b fetchRgb(const unsigned char* color, color_tag) { return Vec3b(color[2], color[1], color[0]); }
|
||||
inline Vec3b fetchRgb(const unsigned char* color, gray_tag) { return Vec3b(color[0], color[0], color[0]); }
|
||||
|
||||
inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); }
|
||||
template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); }
|
||||
|
||||
inline Color vtkcolor(const Color& color)
|
||||
{
|
||||
Color scaled_color = color * (1.0/255.0);
|
||||
std::swap(scaled_color[0], scaled_color[2]);
|
||||
return scaled_color;
|
||||
}
|
||||
|
||||
struct ConvertToVtkImage
|
||||
{
|
||||
struct Impl
|
||||
|
@ -59,9 +59,8 @@ void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj)
|
||||
|
||||
Mat traj;
|
||||
_traj.getMat().convertTo(traj, CV_64F);
|
||||
const Affine3d* dpath = _traj.getMat().ptr<Affine3d>();
|
||||
|
||||
size_t total = _traj.total();
|
||||
const Affine3d* dpath = traj.ptr<Affine3d>();
|
||||
size_t total = traj.total();
|
||||
|
||||
points = vtkSmartPointer<vtkPoints>::New();
|
||||
points->SetDataType(VTK_DOUBLE);
|
||||
@ -81,7 +80,6 @@ void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
cv::Mat cv::viz::vtkTrajectorySource::ExtractPoints(InputArray _traj)
|
||||
{
|
||||
CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT);
|
||||
|
@ -140,14 +140,11 @@ void cv::viz::Widget::setRenderingProperty(int property, double value)
|
||||
{
|
||||
if (!actor->GetMapper()->GetInput()->GetPointData()->GetNormals())
|
||||
{
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkPolyDataMapper::SafeDownCast(actor->GetMapper());
|
||||
vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New();
|
||||
#if VTK_MAJOR_VERSION <= 5
|
||||
normals->SetInput(actor->GetMapper()->GetInput());
|
||||
#else
|
||||
normals->SetInputData(actor->GetMapper()->GetInput());
|
||||
#endif
|
||||
VtkUtils::SetInputData(normals, mapper->GetInput());
|
||||
normals->Update();
|
||||
vtkDataSetMapper::SafeDownCast(actor->GetMapper())->SetInputConnection(normals->GetOutputPort());
|
||||
VtkUtils::SetInputData(mapper, normals->GetOutput());
|
||||
}
|
||||
actor->GetProperty()->SetInterpolationToGouraud();
|
||||
break;
|
||||
@ -156,14 +153,11 @@ void cv::viz::Widget::setRenderingProperty(int property, double value)
|
||||
{
|
||||
if (!actor->GetMapper()->GetInput()->GetPointData()->GetNormals())
|
||||
{
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkPolyDataMapper::SafeDownCast(actor->GetMapper());
|
||||
vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New();
|
||||
#if VTK_MAJOR_VERSION <= 5
|
||||
normals->SetInput(actor->GetMapper()->GetInput());
|
||||
#else
|
||||
normals->SetInputData(actor->GetMapper()->GetInput());
|
||||
#endif
|
||||
VtkUtils::SetInputData(normals, mapper->GetInput());
|
||||
normals->Update();
|
||||
vtkDataSetMapper::SafeDownCast(actor->GetMapper())->SetInputConnection(normals->GetOutputPort());
|
||||
VtkUtils::SetInputData(mapper, normals->GetOutput());
|
||||
}
|
||||
actor->GetProperty()->SetInterpolationToPhong();
|
||||
break;
|
||||
|
@ -78,6 +78,21 @@ namespace cv
|
||||
{
|
||||
return Path::combine(cvtest::TS::ptr()->get_data_path(), "dragon.ply");
|
||||
}
|
||||
|
||||
template<typename _Tp>
|
||||
inline std::vector< Affine3<_Tp> > generate_test_trajectory()
|
||||
{
|
||||
std::vector< Affine3<_Tp> > result;
|
||||
|
||||
for (int i = 0, j = 0; i <= 270; i += 3, j += 10)
|
||||
{
|
||||
double x = 2 * cos(i * 3 * CV_PI/180.0) * (1.0 + 0.5 * cos(1.2 + i * 1.2 * CV_PI/180.0));
|
||||
double y = 0.25 + i/270.0 + sin(j * CV_PI/180.0) * 0.2 * sin(0.6 + j * 1.5 * CV_PI/180.0);
|
||||
double z = 2 * sin(i * 3 * CV_PI/180.0) * (1.0 + 0.5 * cos(1.2 + i * CV_PI/180.0));
|
||||
result.push_back(viz::makeCameraPose(Vec3d(x, y, z), Vec3d::all(0.0), Vec3d(0.0, 1.0, 0.0)));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
@ -48,46 +48,21 @@ TEST(Viz_viz3d, develop)
|
||||
cv::Mat cloud = cv::viz::readCloud(get_dragon_ply_file_path());
|
||||
|
||||
//cv::viz::Mesh3d mesh = cv::viz::Mesh3d::load(get_dragon_ply_file_path());
|
||||
|
||||
//theRNG().fill(mesh.colors, RNG::UNIFORM, 0, 255);
|
||||
|
||||
cv::viz::Viz3d viz("abc");
|
||||
viz.setBackgroundColor(cv::viz::Color::mlab());
|
||||
viz.showWidget("coo", cv::viz::WCoordinateSystem(0.1));
|
||||
|
||||
|
||||
// double c = cos(CV_PI/6);
|
||||
// std::vector<Vec3d> pts;
|
||||
// pts.push_back(Vec3d(0, 0.0, -1.0));
|
||||
// pts.push_back(Vec3d(1, c, -0.5));
|
||||
// pts.push_back(Vec3d(2, c, 0.5));
|
||||
// pts.push_back(Vec3d(3, 0.0, 1.0));
|
||||
// pts.push_back(Vec3d(4, -c, 0.5));
|
||||
// pts.push_back(Vec3d(5, -c, -0.5));
|
||||
|
||||
// viz.showWidget("pl", cv::viz::WPolyLine(Mat(pts), cv::viz::Color::green()));
|
||||
|
||||
//viz.showWidget("pl", cv::viz::WPolyLine(cloud.colRange(0, 100), cv::viz::Color::green()));
|
||||
//viz.spin();
|
||||
|
||||
//cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0));
|
||||
viz.showWidget("coo", cv::viz::WCoordinateSystem(1));
|
||||
|
||||
//viz.showWidget("h", cv::viz::Widget::fromPlyFile("d:/horse-red.ply"));
|
||||
//viz.showWidget("a", cv::viz::WArrow(cv::Point3f(0,0,0), cv::Point3f(1,1,1)));
|
||||
|
||||
std::vector<cv::Affine3d> gt, es;
|
||||
cv::viz::readTrajectory(gt, "d:/Datasets/trajs/gt%05d.xml");
|
||||
//---->>>>> <to_test_in_future>
|
||||
//std::vector<cv::Affine3d> gt, es;
|
||||
//cv::viz::readTrajectory(gt, "d:/Datasets/trajs/gt%05d.xml");
|
||||
//cv::viz::readTrajectory(es, "d:/Datasets/trajs/es%05d.xml");
|
||||
gt.resize(20);
|
||||
|
||||
Affine3d inv = gt[0].inv();
|
||||
for(size_t i = 0; i < gt.size(); ++i)
|
||||
gt[i] = inv * gt[i];
|
||||
|
||||
//viz.showWidget("gt", viz::WTrajectory(gt, viz::WTrajectory::PATH, 1.f, viz::Color::blue()), gt[0].inv());
|
||||
viz.showWidget("gt", viz::WTrajectory(gt, viz::WTrajectory::BOTH, 0.01f, viz::Color::blue()));
|
||||
|
||||
//viz.showWidget("tr", viz::WTrajectory(es, viz::WTrajectory::PATH, 1.f, viz::Color::red()), gt[0].inv());
|
||||
//cv::Mat cloud = cv::viz::readCloud(get_dragon_ply_file_path());
|
||||
//---->>>>> </to_test_in_future>
|
||||
|
||||
//theRNG().fill(colors, cv::RNG::UNIFORM, 0, 255);
|
||||
//viz.showWidget("c", cv::viz::WCloud(cloud, colors));
|
||||
|
@ -156,6 +156,49 @@ TEST(Viz, DISABLED_show_sampled_normals)
|
||||
viz.spin();
|
||||
}
|
||||
|
||||
TEST(Viz, DISABLED_show_trajectories)
|
||||
{
|
||||
std::vector<Affine3d> path = generate_test_trajectory<double>(), sub0, sub1, sub2, sub3, sub4, sub5;
|
||||
|
||||
Mat(path).rowRange(0, path.size()/10+1).copyTo(sub0);
|
||||
Mat(path).rowRange(path.size()/10, path.size()/5+1).copyTo(sub1);
|
||||
Mat(path).rowRange(path.size()/5, 11*path.size()/12).copyTo(sub2);
|
||||
Mat(path).rowRange(11*path.size()/12, path.size()).copyTo(sub3);
|
||||
Mat(path).rowRange(3*path.size()/4, 33*path.size()/40).copyTo(sub4);
|
||||
Mat(path).rowRange(33*path.size()/40, 9*path.size()/10).copyTo(sub5);
|
||||
Matx33d K(1024.0, 0.0, 320.0, 0.0, 1024.0, 240.0, 0.0, 0.0, 1.0);
|
||||
|
||||
Viz3d viz("show_trajectories");
|
||||
viz.showWidget("coos", WCoordinateSystem());
|
||||
viz.showWidget("sub0", WTrajectorySpheres(sub0, 0.25, 0.07));
|
||||
viz.showWidget("sub1", WTrajectory(sub1, WTrajectory::PATH, 0.2, Color::brown()));
|
||||
viz.showWidget("sub2", WTrajectory(sub2, WTrajectory::FRAMES, 0.2));
|
||||
viz.showWidget("sub3", WTrajectory(sub3, WTrajectory::BOTH, 0.2, Color::green()));
|
||||
viz.showWidget("sub4", WTrajectoryFrustums(sub4, K, 0.3));
|
||||
viz.showWidget("sub5", WTrajectoryFrustums(sub5, Vec2d(0.78, 0.78), 0.15));
|
||||
|
||||
int i = 0;
|
||||
while(!viz.wasStopped())
|
||||
{
|
||||
double a = --i % 360;
|
||||
Vec3d pose(sin(a * CV_PI/180), 0.7, cos(a * CV_PI/180));
|
||||
viz.setViewerPose(makeCameraPose(pose * 7.5, Vec3d(0.0, 0.5, 0.0), Vec3d(0.0, 0.1, 0.0)));
|
||||
viz.spinOnce(20, true);
|
||||
}
|
||||
//viz.spin();
|
||||
}
|
||||
|
||||
TEST(Viz, DISABLED_show_trajectory_reposition)
|
||||
{
|
||||
std::vector<Affine3f> path = generate_test_trajectory<float>();
|
||||
|
||||
Viz3d viz("show_trajectory_reposition_to_origin");
|
||||
viz.showWidget("coos", WCoordinateSystem());
|
||||
viz.showWidget("sub3", WTrajectory(Mat(path).rowRange(0, path.size()/3), WTrajectory::BOTH, 0.2, Color::brown()), path.front().inv());
|
||||
viz.spin();
|
||||
}
|
||||
|
||||
|
||||
TEST(Viz, DISABLED_spin_twice_____________________________TODO_UI_BUG)
|
||||
{
|
||||
Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());
|
||||
|
Loading…
x
Reference in New Issue
Block a user