Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

refactored showPointCloud to for shorter code. Implemented NanFilter::copy<T>() function

Browse Source
pull/1453/head
Anatoly Baksheev 12 years ago
parent 1ae5918fa8
commit 1b51ee385b
5 changed files with 95 additions and 100 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      modules/viz/include/opencv2/viz/types.hpp
  2. 111
      modules/viz/src/q/viz3d_impl.hpp
  3. 9
      modules/viz/src/types.cpp
  4. 1
      modules/viz/src/viz3d.cpp
  5. 65
      modules/viz/src/viz3d_impl.cpp

9
modules/viz/include/opencv2/viz/types.hpp
Unescape View File

@ -31,6 +31,10 @@ namespace temp_viz
typedef cv::InputArray InputArray;
using cv::Point3_;
using cv::Vec;
using cv::Mat_;
using cv::DataDepth;
using cv::DataType;
@ -87,9 +91,10 @@ namespace temp_viz
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); }
Vec3d operator*(const Affine3f& affine, const Vec3d& vec);
inline bool isNan(float x)
{

111
modules/viz/src/q/viz3d_impl.hpp
Unescape View File

@ -447,84 +447,65 @@ void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<
void convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m);
template<typename _Tp, typename _Ts, typename _Tc> inline int copy_non_nan_loop(_Tp *d, const Mat& s, const Mat& c)
struct NanFilter
{
CV_Assert(s.size() == c.size());
int j = 0;
for(int y = 0; y < s.rows; ++y)
template<typename _Tp, typename _Msk>
struct Impl
{
const _Ts* srow = s.ptr<_Ts>(y);
const _Tc* crow = c.ptr<_Tc>(y);
for(int x = 0; x < s.cols; ++x)
if (!isNan(crow[x]))
d[j++] = _Tp((srow[x])[0], (srow[x])[1], (srow[x])[2]);
}
return j;
}
typedef Vec<_Tp, 3> _Out;
/** \brief Assign a value to a variable if another variable is not NaN
* \param[in] d the destination variable
* \param[in] s the source variable
* \param[in] c the values to be controlled if NaN (can be different from s)
* \param[out] j number of points that are copied
*/
template<typename _Tp> inline int copy_non_nans(_Tp *d, const Mat& s, const Mat& c)
{
CV_Assert(s.size() == c.size());
int j = 0;
if (s.channels() > 3)
{
if (s.type() == CV_32FC4)
static _Out* copy(const Mat& source, _Out* output, const Mat& nan_mask)
{
switch(c.type())
{
case CV_32FC3: j = copy_non_nan_loop<_Tp, Vec4f, Vec3f>(d, s, c); break;
case CV_32FC4: j = copy_non_nan_loop<_Tp, Vec4f, Vec4f>(d, s, c); break;
case CV_64FC3: j = copy_non_nan_loop<_Tp, Vec4f, Vec3d>(d, s, c); break;
case CV_64FC4: j = copy_non_nan_loop<_Tp, Vec4f, Vec4d>(d, s, c); break;
}
}
else if (s.type() == CV_64FC4)
{
switch(c.type())
CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth());
int s_chs = source.channels();
int m_chs = nan_mask.channels();
for(int y = 0; y < source.rows; ++y)
{
case CV_32FC3: j = copy_non_nan_loop<_Tp, Vec4d, Vec3f>(d, s, c); break;
case CV_32FC4: j = copy_non_nan_loop<_Tp, Vec4d, Vec4f>(d, s, c); break;
case CV_64FC3: j = copy_non_nan_loop<_Tp, Vec4d, Vec3d>(d, s, c); break;
case CV_64FC4: j = copy_non_nan_loop<_Tp, Vec4d, Vec4d>(d, s, c); break;
const _Tp* srow = source.ptr<_Tp>(y);
const _Msk* mrow = nan_mask.ptr<_Msk>(y);
for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
*output++ = _Out(srow);
}
return output;
}
}
else
};
template<typename _Tp>
static inline Vec<_Tp, 3>* copy(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
{
switch(c.type())
{
case CV_32FC3: j = copy_non_nan_loop<_Tp, _Tp, Vec3f>(d, s, c); break;
case CV_32FC4: j = copy_non_nan_loop<_Tp, _Tp, Vec4f>(d, s, c); break;
case CV_64FC3: j = copy_non_nan_loop<_Tp, _Tp, Vec3d>(d, s, c); break;
case CV_64FC4: j = copy_non_nan_loop<_Tp, _Tp, Vec4d>(d, s, c); break;
}
CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F);
typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&);
const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copy, &NanFilter::Impl<_Tp, double>::copy };
return table[nan_mask.depth() - 5](source, output, nan_mask);
}
return j;
}
};
/** \brief Transform points in an array
* \param[in] d the destination variable
* \param[in] lenth the length of the d array
* \param[in] pose affine transform to be applied on each point in d
*/
template<typename _Tp> inline void transform_non_nans(_Tp* d, int length, const Affine3f& pose = Affine3f::Identity())
struct ApplyAffine
{
for (int i = 0; i < length; ++i)
const Affine3f& affine_;
ApplyAffine(const Affine3f& affine) : affine_(affine) {}
template<typename _Tp> Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine * p; }
template<typename _Tp> Vec<_Tp, 3> operator()(const Vec<_Tp, 3>& v)
{
d[i] = pose * d[i];
const float* m = affine_.matrix.val;
Vec<_Tp, 3> result;
result[0] = m[0] * v[0] + m[1] * v[1] + m[ 2] * v[2] + m[ 3];
result[1] = m[4] * v[0] + m[5] * v[1] + m[ 6] * v[2] + m[ 7];
result[2] = m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11];
return result;
}
}
};
}

9
modules/viz/src/types.cpp
Unescape View File

@ -22,12 +22,3 @@ temp_viz::Color temp_viz::Color::white() { return Color(255, 255, 255); }
temp_viz::Color temp_viz::Color::gray() { return Color(128, 128, 128); }
temp_viz::Vec3d temp_viz::operator*(const temp_viz::Affine3f& affine, const temp_viz::Vec3d& vec)
{
const temp_viz::Matx44f& m = affine.matrix;
temp_viz::Vec3d result;
result[0] = m.val[0] * vec[0] + m.val[1] * vec[1] + m.val[ 2] * vec[2] + m.val[ 3];
result[1] = m.val[4] * vec[0] + m.val[5] * vec[1] + m.val[ 6] * vec[2] + m.val[ 7];
result[2] = m.val[8] * vec[0] + m.val[9] * vec[1] + m.val[10] * vec[2] + m.val[11];
return result;
}

1
modules/viz/src/viz3d.cpp
Unescape View File

@ -99,3 +99,4 @@ void temp_viz::Viz3d::registerMouseCallback(void (*callback)(const cv::MouseEven
}
bool temp_viz::Viz3d::wasStopped() const { return impl_->wasStopped(); }

65
modules/viz/src/viz3d_impl.cpp
Unescape View File

@ -2,6 +2,27 @@
#include <q/shapes.h>
#include <q/viz3d_impl.hpp>
namespace temp_viz
{
template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
template<> Vec3f* vtkpoints_data<float>(vtkSmartPointer<vtkPoints>& points)
{
CV_Assert(points->GetDataType() == VTK_FLOAT);
vtkDataArray *data = points->GetData();
float *pointer = static_cast<vtkFloatArray*>(data)->GetPointer(0);
return reinterpret_cast<Vec3f*>(pointer);
}
template<> Vec3d* vtkpoints_data<double>(vtkSmartPointer<vtkPoints>& points)
{
CV_Assert(points->GetDataType() == VTK_DOUBLE);
vtkDataArray *data = points->GetData();
double *pointer = static_cast<vtkDoubleArray*>(data)->GetPointer(0);
return reinterpret_cast<Vec3d*>(pointer);
}
}
void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
{
if (window_)
@ -21,14 +42,14 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
{
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(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;
vtkIdType nr_points = cloud.total();
// If the cloud already exists, update otherwise create new one
CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
@ -41,7 +62,6 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
vertices = vtkSmartPointer<vtkCellArray>::New ();
polydata->SetVerts (vertices);
nr_points = cloud.total();
points = polydata->GetPoints ();
if (!points)
@ -67,29 +87,28 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
points->SetDataTypeToFloat ();
else if (cloud.depth() == CV_64F)
points->SetDataTypeToDouble ();
// Copy the new point array in
nr_points = cloud.total();
points->SetNumberOfPoints (nr_points);
}
int j = 0;
if (cloud.depth() == CV_32F)
{
// Get a pointer to the beginning of the data array
Vec3f *data = reinterpret_cast<Vec3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0));
j = copy_non_nans(data, cloud, cloud);
transform_non_nans(data,j,pose);
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 = reinterpret_cast<Vec3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
j = copy_non_nans(data, cloud, cloud);
transform_non_nans(data,j,pose);
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;
}
nr_points = j;
points->SetNumberOfPoints (nr_points);
vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
@ -102,19 +121,17 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
// Set the cells and the vertices
vertices->SetCells (nr_points, cells);
// Get the colors from the handler
Vec2d minmax;
vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
scalars->SetNumberOfComponents (3);
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
// Get a random color
Vec3b* colors_data = new Vec3b[nr_points];
j = copy_non_nans(colors_data, colors, cloud);
NanFilter::copy(colors, colors_data, cloud);
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(colors_data), 3 * nr_points, 0);
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);
@ -133,8 +150,8 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
(*cloud_actor_map_)[id].actor = actor;
(*cloud_actor_map_)[id].cells = initcells;
const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
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();

Write Preview
Loading…
Cancel
Save