36 #include <visp3/core/vpCPUFeatures.h> 37 #include <visp3/mbt/vpMbtFaceDepthDense.h> 39 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) 40 #include <emmintrin.h> 41 #define VISP_HAVE_SSE2 1 44 #define USE_SSE_CODE 1 45 #if VISP_HAVE_SSE2 && USE_SSE_CODE 52 : m_cam(), m_clippingFlag(
vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(NULL),
53 m_planeObject(), m_polygon(NULL), m_useScanLine(false),
54 m_depthDenseFilteringMethod(DEPTH_OCCUPANCY_RATIO_FILTERING), m_depthDenseFilteringMaxDist(3.0),
55 m_depthDenseFilteringMinDist(0.8), m_depthDenseFilteringOccupancyRatio(0.3), m_isTrackedDepthDenseFace(true),
56 m_isVisible(false), m_listOfFaceLines(), m_planeCamera(), m_pointCloudFace(), m_polygonLines()
84 PolygonLine polygon_line;
87 polygon_line.m_poly.setNbPoint(2);
88 polygon_line.m_poly.addPoint(0, P1);
89 polygon_line.m_poly.addPoint(1, P2);
95 polygon_line.m_p1 = &polygon_line.m_poly.p[0];
96 polygon_line.m_p2 = &polygon_line.m_poly.p[1];
101 bool already_here =
false;
142 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
143 const unsigned int stepX,
const unsigned int stepY
144 #
if DEBUG_DISPLAY_DEPTH_DENSE
147 std::vector<std::vector<vpImagePoint> > &roiPts_vec
152 unsigned int width = point_cloud->width, height = point_cloud->height;
155 if (point_cloud->width == 0 || point_cloud->height == 0)
158 std::vector<vpImagePoint> roiPts;
159 double distanceToFace;
161 #
if DEBUG_DISPLAY_DEPTH_DENSE
168 if (roiPts.size() <= 2) {
170 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
183 unsigned int top = (
unsigned int)std::max(0.0, bb.
getTop());
184 unsigned int bottom = (
unsigned int)std::min((
double)height, std::max(0.0, bb.
getBottom()));
185 unsigned int left = (
unsigned int)std::max(0.0, bb.
getLeft());
186 unsigned int right = (
unsigned int)std::min((
double)width, std::max(0.0, bb.
getRight()));
204 double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
207 int totalTheoreticalPoints = 0, totalPoints = 0;
208 for (
unsigned int i = top; i < bottom; i += stepY) {
209 for (
unsigned int j = left; j < right; j += stepX) {
210 if ((
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
211 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
214 totalTheoreticalPoints++;
216 if (
vpMeTracker::inMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0) {
223 prev_x = (*point_cloud)(j, i).x;
224 prev_y = (*point_cloud)(j, i).y;
225 prev_z = (*point_cloud)(j, i).z;
244 #if DEBUG_DISPLAY_DEPTH_DENSE 245 debugImage[i][j] = 255;
253 if (checkSSE2 && push) {
270 const unsigned int height,
const std::vector<vpColVector> &point_cloud,
271 const unsigned int stepX,
const unsigned int stepY
272 #
if DEBUG_DISPLAY_DEPTH_DENSE
275 std::vector<std::vector<vpImagePoint> > &roiPts_vec
282 if (width == 0 || height == 0)
285 std::vector<vpImagePoint> roiPts;
286 double distanceToFace;
288 #
if DEBUG_DISPLAY_DEPTH_DENSE
295 if (roiPts.size() <= 2) {
297 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
310 unsigned int top = (
unsigned int)std::max(0.0, bb.
getTop());
311 unsigned int bottom = (
unsigned int)std::min((
double)height, std::max(0.0, bb.
getBottom()));
312 unsigned int left = (
unsigned int)std::max(0.0, bb.
getLeft());
313 unsigned int right = (
unsigned int)std::min((
double)width, std::max(0.0, bb.
getRight()));
327 double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
330 int totalTheoreticalPoints = 0, totalPoints = 0;
331 for (
unsigned int i = top; i < bottom; i += stepY) {
332 for (
unsigned int j = left; j < right; j += stepX) {
333 if ((
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
334 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
337 totalTheoreticalPoints++;
346 prev_x = point_cloud[i * width + j][0];
347 prev_y = point_cloud[i * width + j][1];
348 prev_z = point_cloud[i * width + j][2];
367 #if DEBUG_DISPLAY_DEPTH_DENSE 368 debugImage[i][j] = 255;
376 if (checkSSE2 && push) {
400 bool isvisible =
false;
404 int index = *itindex;
457 double *ptr_L = L.
data;
458 double *ptr_error = error.
data;
460 const __m128d vnx = _mm_set1_pd(nx);
461 const __m128d vny = _mm_set1_pd(ny);
462 const __m128d vnz = _mm_set1_pd(nz);
463 const __m128d vd = _mm_set1_pd(D);
465 double tmp_a1[2], tmp_a2[2], tmp_a3[2];
467 for (; cpt <=
m_pointCloudFace.size() - 6; cpt += 6, ptr_point_cloud += 6) {
468 const __m128d vx = _mm_loadu_pd(ptr_point_cloud);
469 const __m128d vy = _mm_loadu_pd(ptr_point_cloud + 2);
470 const __m128d vz = _mm_loadu_pd(ptr_point_cloud + 4);
472 const __m128d va1 = _mm_sub_pd(_mm_mul_pd(vnz, vy), _mm_mul_pd(vny, vz));
473 const __m128d va2 = _mm_sub_pd(_mm_mul_pd(vnx, vz), _mm_mul_pd(vnz, vx));
474 const __m128d va3 = _mm_sub_pd(_mm_mul_pd(vny, vx), _mm_mul_pd(vnx, vy));
476 _mm_storeu_pd(tmp_a1, va1);
477 _mm_storeu_pd(tmp_a2, va2);
478 _mm_storeu_pd(tmp_a3, va3);
506 const __m128d verror =
507 _mm_add_pd(_mm_add_pd(vd, _mm_mul_pd(vnx, vx)), _mm_add_pd(_mm_mul_pd(vny, vy), _mm_mul_pd(vnz, vz)));
508 _mm_storeu_pd(ptr_error, verror);
518 double _a1 = (nz * y) - (ny * z);
519 double _a2 = (nx * z) - (nz * x);
520 double _a3 = (ny * x) - (nx * y);
523 L[(
unsigned int)(cpt / 3)][0] = nx;
524 L[(
unsigned int)(cpt / 3)][1] = ny;
525 L[(
unsigned int)(cpt / 3)][2] = nz;
526 L[(
unsigned int)(cpt / 3)][3] = _a1;
527 L[(
unsigned int)(cpt / 3)][4] = _a2;
528 L[(
unsigned int)(cpt / 3)][5] = _a3;
541 error[(
unsigned int)(cpt / 3)] = D + (normal.
t() * pt);
551 unsigned int idx = 0;
557 double _a1 = (nz * y) - (ny * z);
558 double _a2 = (nx * z) - (nz * x);
559 double _a3 = (ny * x) - (nx * y);
573 error[idx] = D + (normal.
t() * pt);
579 const unsigned int height, std::vector<vpImagePoint> &roiPts
580 #
if DEBUG_DISPLAY_DEPTH_DENSE
582 std::vector<std::vector<vpImagePoint> > &roiPts_vec
585 double &distanceToFace)
592 it->m_p1->changeFrame(cMo);
593 it->m_p2->changeFrame(cMo);
597 it->m_poly.changeFrame(cMo);
598 it->m_poly.computePolygonClipped(
m_cam);
600 if (it->m_poly.polyClipped.size() == 2 &&
608 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
614 for (
unsigned int i = 0; i < linesLst.size(); i++) {
616 linesLst[i].second.project();
624 roiPts.push_back(ip1);
625 roiPts.push_back(ip2);
627 faceCentroid.
set_X(faceCentroid.
get_X() + linesLst[i].first.get_X() + linesLst[i].second.get_X());
628 faceCentroid.
set_Y(faceCentroid.
get_Y() + linesLst[i].first.get_Y() + linesLst[i].second.get_Y());
629 faceCentroid.
set_Z(faceCentroid.
get_Z() + linesLst[i].first.get_Z() + linesLst[i].second.get_Z());
631 #if DEBUG_DISPLAY_DEPTH_DENSE 632 std::vector<vpImagePoint> roiPts_;
633 roiPts_.push_back(ip1);
634 roiPts_.push_back(ip2);
635 roiPts_vec.push_back(roiPts_);
639 if (linesLst.empty()) {
640 distanceToFace = std::numeric_limits<double>::max();
642 faceCentroid.
set_X(faceCentroid.
get_X() / (2 * linesLst.size()));
643 faceCentroid.
set_Y(faceCentroid.
get_Y() / (2 * linesLst.size()));
644 faceCentroid.
set_Z(faceCentroid.
get_Z() / (2 * linesLst.size()));
647 sqrt(faceCentroid.
get_X() * faceCentroid.
get_X() + faceCentroid.
get_Y() * faceCentroid.
get_Y() +
657 std::vector<vpPoint> polygonsClipped;
660 if (polygonsClipped.empty()) {
661 distanceToFace = std::numeric_limits<double>::max();
665 for (
size_t i = 0; i < polygonsClipped.size(); i++) {
666 faceCentroid.
set_X(faceCentroid.
get_X() + polygonsClipped[i].get_X());
667 faceCentroid.
set_Y(faceCentroid.
get_Y() + polygonsClipped[i].get_Y());
668 faceCentroid.
set_Z(faceCentroid.
get_Z() + polygonsClipped[i].get_Z());
671 faceCentroid.
set_X(faceCentroid.
get_X() / polygonsClipped.size());
672 faceCentroid.
set_Y(faceCentroid.
get_Y() / polygonsClipped.size());
673 faceCentroid.
set_Z(faceCentroid.
get_Z() / polygonsClipped.size());
675 distanceToFace = sqrt(faceCentroid.
get_X() * faceCentroid.
get_X() + faceCentroid.
get_Y() * faceCentroid.
get_Y() +
679 #if DEBUG_DISPLAY_DEPTH_DENSE 680 roiPts_vec.push_back(roiPts);
687 const bool displayFullModel)
695 line->
display(I, cMo, cam, col, thickness, displayFullModel);
702 const bool displayFullModel)
710 line->
display(I, cMo, cam, col, thickness, displayFullModel);
742 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
743 dz <= std::numeric_limits<double>::epsilon())
755 (*it)->setCameraParameters(camera);
765 (*it)->useScanLine = v;
unsigned int getNbFeatures() const
Implementation of a matrix and operations on matrices.
double m_distNearClip
Distance for near clipping.
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
Implements a 3D polygon with render functionnalities like clipping.
void setVisible(bool _isvisible)
std::vector< double > m_pointCloudFace
List of depth points inside the face.
vpCameraParameters m_cam
Camera intrinsic parameters.
Implementation of an homogeneous matrix and operations on such kind of matrices.
int m_depthDenseFilteringMethod
Method to use to consider or not the face.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void setFarClippingDistance(const double &dist)
void setCameraParameters(const vpCameraParameters &camera)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Class to define colors available for display functionnalities.
bool m_useScanLine
Scan line visibility.
vpPoint * p1
The first extremity.
double get_oY() const
Get the point Y coordinate in the object frame.
vpMbScanLine & getMbScanLineRenderer()
Manage the line of a polygon used in the model-based tracker.
Type * data
Address of the first element of the data array.
double m_depthDenseFilteringMinDist
Minimum distance threshold.
void set_X(const double X)
Set the point X coordinate in the camera frame.
vpMbtPolygon & getPolygon()
bool m_isTrackedDepthDenseFace
Flag to define if the face should be tracked or not.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
static bool inMask(const vpImage< bool > *mask, const unsigned int i, const unsigned int j)
Class that defines what is a point.
bool m_isVisible
Visibility flag.
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPlane m_planeObject
Plane equation described in the object frame.
Defines a generic 2D polygon.
void computeVisibilityDisplay()
vpRect getBoundingBox() const
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
virtual ~vpMbtFaceDepthDense()
unsigned int m_clippingFlag
Flags specifying which clipping to used.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
vpPoint * p2
The second extremity.
void changeFrame(const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
VISP_EXPORT bool checkSSE2()
double m_distFarClip
Distance for near clipping.
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
double m_depthDenseFilteringOccupancyRatio
Ratio between available depth points and theoretical number of points.
void setIndex(const unsigned int i)
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
void setScanLineVisibilityTest(const bool v)
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
double get_Y() const
Get the point Y coordinate in the camera frame.
double m_depthDenseFilteringMaxDist
Maximum distance threshold.
void setClipping(const unsigned int &flags)
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
double get_Z() const
Get the point Z coordinate in the camera frame.
void setName(const std::string &line_name)
void setCameraParameters(const vpCameraParameters &camera)
double get_oX() const
Get the point X coordinate in the object frame.
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
Implementation of column vector and the associated operations.
void setRight(double pos)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool isVisible(const unsigned int i)
std::vector< PolygonLine > m_polygonLines
Polygon lines used for scan-line visibility.
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
Defines a rectangle in the plane.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
double get_X() const
Get the point X coordinate in the camera frame.
void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, std::vector< vpImagePoint > &roiPts, double &distanceToFace)
void setBottom(double pos)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
void resize(const unsigned int i, const bool flagNullify=true)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
void computeFov(const unsigned int &w, const unsigned int &h)