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

implemented copy_non_nans, isnan

Browse Source
This commit is contained in:
ozantonkal 2013-06-11 10:48:48 +02:00
parent c4fe98977a
commit abdc022b20
4 changed files with 108 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
modules/viz/include/opencv2/viz/types.hpp
View File

@ -14,7 +14,9 @@ namespace temp_viz
typedef std::string String; typedef std::string String;
typedef cv::Vec3d Vec3d; typedef cv::Vec3d Vec3d;
typedef cv::Vec3f Vec3f;
typedef cv::Vec4d Vec4d; typedef cv::Vec4d Vec4d;
typedef cv::Vec4f Vec4f;
typedef cv::Vec2d Vec2d; typedef cv::Vec2d Vec2d;
typedef cv::Vec2i Vec2i; typedef cv::Vec2i Vec2i;
typedef cv::Vec3b Vec3b; typedef cv::Vec3b Vec3b;
@ -86,18 +88,19 @@ namespace temp_viz
template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); } template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); }
Point3d operator*(const Affine3f& affine, const Point3d& point); Vec3d operator*(const Affine3f& affine, const Vec3d& vec);
inline bool isNaN( float x ) inline bool isNaN( float x )
{ {
union { float f; unsigned int u; } v = { x }; unsigned int *u = (reinterpret_cast<unsigned int *>(&x));
return ((v.u & 0x7f800000) == 0x7f800000) && (v.u & 0x007fffff); return ((u[0] & 0x7f800000) == 0x7f800000) && (u[0] & 0x007fffff);
} }
inline bool isNaN( double x ) inline bool isNaN( double x )
{ {
union { double d; unsigned int u[2]; } v = { x }; // Here u has two elements
return (v.u[1] & 0x7ff00000) == 0x7ff00000 && unsigned int *u = (reinterpret_cast<unsigned int *>(&x));
(v.u[0] != 0 || (v.u[1] & 0x000fffff) != 0); return (u[1] & 0x7ff00000) == 0x7ff00000 &&
(u[0] != 0 || (u[1] & 0x000fffff) != 0);
} }
} }

82
modules/viz/src/q/viz3d_impl.hpp
View File

@ -446,6 +446,88 @@ void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer<vtkMatrix4x4> &vt
void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix); void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
void convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m); 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, InputArray _s, InputArray _c)
{
Mat s = _s.getMat();
Mat c = _c.getMat();
CV_Assert(s.size() == c.size());
int j = 0;
for(int y = 0; y < s.rows; ++y)
{
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][0]) && !isNaN(crow[x][1]) && !isNaN(crow[x][2]))
{
d[j++] = _Tp((srow[x])[0], (srow[x])[1], (srow[x])[2]);
}
}
return j;
}
/** \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, InputArray _s, InputArray _c)
{
Mat s = _s.getMat();
Mat c = _c.getMat();
CV_Assert(s.size() == c.size());
int j = 0;
if (s.channels() > 3)
{
if (s.type() == CV_32FC4)
{
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())
{
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;
}
/** \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 i = 0; i < length; ++i)
{
d[i] = pose * d[i];
}
}
} }

10
modules/viz/src/types.cpp
View File

@ -22,12 +22,12 @@ 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::Color temp_viz::Color::gray() { return Color(128, 128, 128); }
temp_viz::Point3d temp_viz::operator*(const temp_viz::Affine3f& affine, const temp_viz::Point3d& point) temp_viz::Vec3d temp_viz::operator*(const temp_viz::Affine3f& affine, const temp_viz::Vec3d& vec)
{ {
const temp_viz::Matx44f& m = affine.matrix; const temp_viz::Matx44f& m = affine.matrix;
temp_viz::Point3d result; temp_viz::Vec3d result;
result.x = m.val[0] * point.x + m.val[1] * point.y + m.val[ 2] * point.z + m.val[ 3]; result[0] = m.val[0] * vec[0] + m.val[1] * vec[1] + m.val[ 2] * vec[2] + m.val[ 3];
result.y = m.val[4] * point.x + m.val[5] * point.y + m.val[ 6] * point.z + m.val[ 7]; result[1] = m.val[4] * vec[0] + m.val[5] * vec[1] + m.val[ 6] * vec[2] + m.val[ 7];
result.z = m.val[8] * point.x + m.val[9] * point.y + m.val[10] * point.z + m.val[11]; result[2] = m.val[8] * vec[0] + m.val[9] * vec[1] + m.val[10] * vec[2] + m.val[11];
return result; return result;
} }

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

@ -17,14 +17,13 @@ void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name)
void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); } void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); } void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, InputArray _colors, const Affine3f& pose) void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, InputArray _colors, const Affine3f& pose)
{ {
Mat cloud = _cloud.getMat(); Mat cloud = _cloud.getMat();
Mat colors = _colors.getMat(); Mat colors = _colors.getMat();
CV_Assert((cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3)); 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()); CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size());
vtkSmartPointer<vtkPolyData> polydata; vtkSmartPointer<vtkPolyData> polydata;
vtkSmartPointer<vtkCellArray> vertices; vtkSmartPointer<vtkCellArray> vertices;
vtkSmartPointer<vtkPoints> points; vtkSmartPointer<vtkPoints> points;
@ -74,35 +73,20 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
} }
int j = 0; int j = 0;
if (cloud.type() == CV_32FC3)
if (cloud.depth() == CV_32F)
{ {
// Get a pointer to the beginning of the data array // Get a pointer to the beginning of the data array
Point3f *data = reinterpret_cast<Point3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0)); Vec3f *data = reinterpret_cast<Vec3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0));
j = copy_non_nans(data, cloud, cloud);
// Scan through the data and apply mask where point is NAN transform_non_nans(data,j,pose);
for(int y = 0; y < cloud.rows; ++y)
{
const Point3f* crow = cloud.ptr<Point3f>(y);
for(int x = 0; x < cloud.cols; ++x)
//TODO implementa templated copy_if() or copy_non_nans() and use everywhere.
if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
data[j++] = pose * crow[x];
}
} }
else if (cloud.type() == CV_64FC3) else if (cloud.depth() == CV_64F)
{ {
// Get a pointer to the beginning of the data array // Get a pointer to the beginning of the data array
Point3d *data = reinterpret_cast<Point3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0)); Vec3d *data = reinterpret_cast<Vec3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
j = copy_non_nans(data, cloud, cloud);
// If a point is NaN, ignore it transform_non_nans(data,j,pose);
for(int y = 0; y < cloud.rows; ++y)
{
const Point3d* crow = cloud.ptr<Point3d>(y);
for(int x = 0; x < cloud.cols; ++x)
if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
data[j++] = pose * crow[x];
}
} }
nr_points = j; nr_points = j;
@ -126,32 +110,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
// Get a random color // Get a random color
Vec3b* colors_data = new Vec3b[nr_points]; Vec3b* colors_data = new Vec3b[nr_points];
j = copy_non_nans(colors_data, colors, cloud);
j = 0;
if (cloud.type() == CV_32FC3)
{
for(int y = 0; y < colors.rows; ++y)
{
const Vec3b* crow = colors.ptr<Vec3b>(y);
const Point3f* cloud_row = cloud.ptr<Point3f>(y);
for(int x = 0; x < colors.cols; ++x)
if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1)
colors_data[j++] = crow[x];
}
}
else if (cloud.type() == CV_64FC3)
{
for(int y = 0; y < colors.rows; ++y)
{
const Vec3b* crow = colors.ptr<Vec3b>(y);
const Point3d* cloud_row = cloud.ptr<Point3d>(y);
for(int x = 0; x < colors.cols; ++x)
if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1)
colors_data[j++] = crow[x];
}
}
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(colors_data), 3 * nr_points, 0); reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(colors_data), 3 * nr_points, 0);