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

Merge pull request #6 from ozantonkal/implementing_addPointCloud

Browse Source 
Implementing add point cloud
pull/1453/head
Anatoly Baksheev 12 years ago
parent c622ebf890 abdc022b20
commit da32ca4948
4 changed files with 122 additions and 53 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. 19
      modules/viz/include/opencv2/viz/types.hpp
  2. 82
      modules/viz/src/q/viz3d_impl.hpp
  3. 9
      modules/viz/src/types.cpp
  4. 65
      modules/viz/src/viz3d_impl.cpp

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

@ -1,6 +1,7 @@
#pragma once
#include <vector>
#include <boost/concept_check.hpp>
#include <opencv2/core/cvdef.h>
#include <opencv2/core.hpp>
#include <opencv2/core/affine.hpp>
@ -13,7 +14,9 @@ namespace temp_viz
typedef std::string String;
typedef cv::Vec3d Vec3d;
typedef cv::Vec3f Vec3f;
typedef cv::Vec4d Vec4d;
typedef cv::Vec4f Vec4f;
typedef cv::Vec2d Vec2d;
typedef cv::Vec2i Vec2i;
typedef cv::Vec3b Vec3b;
@ -84,4 +87,20 @@ 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 )
{
unsigned int *u = (reinterpret_cast<unsigned int *>(&x));
return ((u[0] & 0x7f800000) == 0x7f800000) && (u[0] & 0x007fffff);
}
inline bool isNaN( double x )
{
// Here u has two elements
unsigned int *u = (reinterpret_cast<unsigned int *>(&x));
return (u[1] & 0x7ff00000) == 0x7ff00000 &&
(u[0] != 0 || (u[1] & 0x000fffff) != 0);
}
}

82
modules/viz/src/q/viz3d_impl.hpp
Unescape 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 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];
}
}
}

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

@ -22,3 +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::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;
}

65
modules/viz/src/viz3d_impl.cpp
Unescape 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::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)
{
Mat cloud = _cloud.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());
vtkSmartPointer<vtkPolyData> polydata;
vtkSmartPointer<vtkCellArray> vertices;
vtkSmartPointer<vtkPoints> points;
@ -74,35 +73,20 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
}
int j = 0;
if (cloud.type() == CV_32FC3)
if (cloud.depth() == CV_32F)
{
// Get a pointer to the beginning of the data array
Point3f *data = reinterpret_cast<Point3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0));
// Scan through the data and apply mask where point is NAN
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];
}
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);
}
else if (cloud.type() == CV_64FC3)
else if (cloud.depth() == CV_64F)
{
// Get a pointer to the beginning of the data array
Point3d *data = reinterpret_cast<Point3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
// If a point is NaN, ignore it
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];
}
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);
}
nr_points = j;
@ -126,32 +110,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
// Get a random color
Vec3b* colors_data = new Vec3b[nr_points];
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];
}
}
j = copy_non_nans(colors_data, colors, cloud);
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(colors_data), 3 * nr_points, 0);

Write Preview
Loading…
Cancel
Save