generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source
This commit is contained in:
Anatoly Baksheev 2013-06-11 21:30:19 +04:00
parent 1ae5918fa8
commit 1b51ee385b
5 changed files with 99 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
modules/viz/include/opencv2/viz/types.hpp
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)
{

119
modules/viz/src/q/viz3d_impl.hpp
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())
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, 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())
{
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;
}
}
}
else
{
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;
}
}
return j;
}
const _Tp* srow = source.ptr<_Tp>(y);
const _Msk* mrow = nan_mask.ptr<_Msk>(y);
/** \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())
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;
}
};
template<typename _Tp>
static inline Vec<_Tp, 3>* copy(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
{
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);
}
};
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
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
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
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();