From 2cfc35317260f57caec5a7ed4df71f43cda028f3 Mon Sep 17 00:00:00 2001 From: Hamdi Sahloul Date: Fri, 24 Feb 2017 01:16:16 +0900 Subject: [PATCH] Re-write surface matching using vectors and matrices --- .../opencv2/surface_matching/pose_3d.hpp | 30 +- .../opencv2/surface_matching/ppf_helpers.hpp | 29 +- .../opencv2/surface_matching/ppf_match_3d.hpp | 8 +- .../opencv2/surface_matching/t_hash_int.hpp | 8 +- modules/surface_matching/src/c_utils.hpp | 609 ++++-------------- .../surface_matching/src/hash_murmur64.hpp | 36 +- .../surface_matching/src/hash_murmur86.hpp | 52 +- modules/surface_matching/src/icp.cpp | 137 ++-- modules/surface_matching/src/pose_3d.cpp | 141 +--- modules/surface_matching/src/ppf_helpers.cpp | 239 ++----- modules/surface_matching/src/ppf_match_3d.cpp | 248 +++---- modules/surface_matching/src/t_hash_int.cpp | 118 ++-- 12 files changed, 506 insertions(+), 1149 deletions(-) diff --git a/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp b/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp index cd9c4a422..82984bf34 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp +++ b/modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp @@ -77,42 +77,40 @@ public: numVotes=0; residual = 0; - for (int i=0; i<16; i++) - pose[i]=0; + pose = Matx44d::all(0); } - Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0) + Pose3D(double Alpha, size_t ModelIndex=0, size_t NumVotes=0) { alpha = Alpha; modelIndex = ModelIndex; numVotes = NumVotes; residual=0; - for (int i=0; i<16; i++) - pose[i]=0; + pose = Matx44d::all(0); } /** * \brief Updates the pose with the new one * \param [in] NewPose New pose to overwrite */ - void updatePose(double NewPose[16]); + void updatePose(Matx44d& NewPose); /** * \brief Updates the pose with the new one */ - void updatePose(double NewR[9], double NewT[3]); + void updatePose(Matx33d& NewR, Vec3d& NewT); /** * \brief Updates the pose with the new one, but this time using quaternions to represent rotation */ - void updatePoseQuat(double Q[4], double NewT[3]); + void updatePoseQuat(Vec4d& Q, Vec3d& NewT); /** * \brief Left multiplies the existing pose in order to update the transformation * \param [in] IncrementalPose New pose to apply */ - void appendPose(double IncrementalPose[16]); + void appendPose(Matx44d& IncrementalPose); void printPose(); Pose3DPtr clone(); @@ -125,17 +123,19 @@ public: virtual ~Pose3D() {} double alpha, residual; - unsigned int modelIndex; - unsigned int numVotes; - double pose[16], angle, t[3], q[4]; + size_t modelIndex, numVotes; + Matx44d pose; + double angle; + Vec3d t; + Vec4d q; }; /** -* @brief When multiple poses (see Pose3D) are grouped together (contribute to the same transformation) +* @brief When multiple poses (see Pose3D) are grouped together (contribute to the same transformation) * pose clusters occur. This class is a general container for such groups of poses. It is possible to store, * load and perform IO on these poses. */ -class CV_EXPORTS PoseCluster3D +class CV_EXPORTS_W PoseCluster3D { public: PoseCluster3D() @@ -175,7 +175,7 @@ public: int readPoseCluster(const std::string& FileName); std::vector poseList; - int numVotes; + size_t numVotes; int id; }; diff --git a/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp b/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp index 1dc00dfac..7b4de88de 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp +++ b/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp @@ -61,14 +61,14 @@ namespace ppf_match_3d * and whether it should be loaded or not * @return Returns the matrix on successfull load */ -CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals = 0); +CV_EXPORTS_W Mat loadPLYSimple(const char* fileName, int withNormals = 0); /** * @brief Write a point cloud to PLY file * @param [in] PC Input point cloud * @param [in] fileName The PLY model file to write */ -CV_EXPORTS void writePLY(Mat PC, const char* fileName); +CV_EXPORTS_W void writePLY(Mat PC, const char* fileName); /** * @brief Used for debbuging pruposes, writes a point cloud to a PLY file with the tip @@ -76,7 +76,7 @@ CV_EXPORTS void writePLY(Mat PC, const char* fileName); * @param [in] PC Input point cloud * @param [in] fileName The PLY model file to write */ -CV_EXPORTS void writePLYVisibleNormals(Mat PC, const char* fileName); +CV_EXPORTS_W void writePLYVisibleNormals(Mat PC, const char* fileName); Mat samplePCUniform(Mat PC, int sampleStep); Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector& indices); @@ -94,26 +94,15 @@ Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector& indices); * by the distance to the origin. This parameter enables/disables the use of weighting. * @return Sampled point cloud */ -CV_EXPORTS Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrange[2], float sample_step_relative, int weightByCenter=0); +CV_EXPORTS_W Mat samplePCByQuantization(Mat pc, Vec2f& xrange, Vec2f& yrange, Vec2f& zrange, float sample_step_relative, int weightByCenter=0); -void computeBboxStd(Mat pc, float xRange[2], float yRange[2], float zRange[2]); +void computeBboxStd(Mat pc, Vec2f& xRange, Vec2f& yRange, Vec2f& zRange); void* indexPCFlann(Mat pc); void destroyFlann(void* flannIndex); void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances); void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors); -/** - * Mostly for visualization purposes. Normalizes the point cloud in a Hartley-Zissermann - * fashion. In other words, the point cloud is centered, and scaled such that the largest - * distance from the origin is sqrt(2). Finally a rescaling is applied. - * @param [in] pc Input point cloud (CV_32F family). Point clouds with 3 or 6 elements per - * row are expected. - * @param [in] scale The scale after normalization. Default to 1. - * @return Normalized point cloud -*/ -CV_EXPORTS Mat normalize_pc(Mat pc, float scale); - Mat normalizePCCoeff(Mat pc, float scale, float* Cx, float* Cy, float* Cz, float* MinVal, float* MaxVal); Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal, float MaxVal); @@ -125,20 +114,20 @@ Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal * @param [in] Pose 4x4 pose matrix, but linearized in row-major form. * @return Transformed point cloud */ -CV_EXPORTS Mat transformPCPose(Mat pc, const double Pose[16]); +CV_EXPORTS_W Mat transformPCPose(Mat pc, const Matx44d& Pose); /** * Generate a random 4x4 pose matrix * @param [out] Pose The random pose */ -CV_EXPORTS void getRandomPose(double Pose[16]); +CV_EXPORTS_W void getRandomPose(Matx44d& Pose); /** * Adds a uniform noise in the given scale to the input point cloud * @param [in] pc Input point cloud (CV_32F family). * @param [in] scale Input scale of the noise. The larger the scale, the more noisy the output */ -CV_EXPORTS Mat addNoisePC(Mat pc, double scale); +CV_EXPORTS_W Mat addNoisePC(Mat pc, double scale); /** * @brief Compute the normals of an arbitrary point cloud @@ -154,7 +143,7 @@ CV_EXPORTS Mat addNoisePC(Mat pc, double scale); * @param [in] viewpoint * @return Returns 0 on success */ -CV_EXPORTS_W int computeNormalsPC3d(const Mat& PC, CV_OUT Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3d& viewpoint); +CV_EXPORTS_W int computeNormalsPC3d(const Mat& PC, CV_OUT Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3f& viewpoint); //! @} diff --git a/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp b/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp index c72ef7bb5..8e6861065 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp +++ b/modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp @@ -94,7 +94,7 @@ typedef struct THash * detector.match(pcTest, results, 1.0/5.0,0.05); * @endcode */ -class CV_EXPORTS PPF3DDetector +class CV_EXPORTS_W PPF3DDetector { public: @@ -160,9 +160,9 @@ protected: void clearTrainingModels(); private: - void computePPFFeatures(const double p1[4], const double n1[4], - const double p2[4], const double n2[4], - double f[4]); + void computePPFFeatures(const Vec3d& p1, const Vec3d& n1, + const Vec3d& p2, const Vec3d& n2, + Vec4d& f); bool matchPose(const Pose3D& sourcePose, const Pose3D& targetPose); diff --git a/modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp b/modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp index 9e251e25b..4e9b5bbe2 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp +++ b/modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp @@ -55,7 +55,7 @@ namespace ppf_match_3d //! @addtogroup surface_matching //! @{ -typedef unsigned int KeyType; +typedef uint KeyType; typedef struct hashnode_i { @@ -68,7 +68,7 @@ typedef struct HSHTBL_i { size_t size; struct hashnode_i **nodes; - size_t (*hashfunc)(unsigned int); + size_t (*hashfunc)(uint); } hashtable_int; @@ -76,7 +76,7 @@ typedef struct HSHTBL_i from http://www-graphics.stanford.edu/~seander/bithacks.html */ -inline static unsigned int next_power_of_two(unsigned int value) +inline static uint next_power_of_two(uint value) { --value; @@ -90,7 +90,7 @@ inline static unsigned int next_power_of_two(unsigned int value) return value; } -hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int)); +hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(uint)); void hashtableDestroy(hashtable_int *hashtbl); int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data); int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data); diff --git a/modules/surface_matching/src/c_utils.hpp b/modules/surface_matching/src/c_utils.hpp index 6ef202699..1ed2e9f5b 100644 --- a/modules/surface_matching/src/c_utils.hpp +++ b/modules/surface_matching/src/c_utils.hpp @@ -55,35 +55,23 @@ const float EPS = 1.192092896e-07F; /* smallest such that 1.0+FLT_EPSILON #define M_PI 3.1415926535897932384626433832795 #endif -static inline double TNorm3(const double v[]) +static inline void TNormalize3(Vec3d& v) { - return (sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2])); -} - -static inline void TNormalize3(double v[]) -{ - double normTemp=TNorm3(v); - if (normTemp>0) + double norm = cv::norm(v); + if (norm > EPS) { - v[0]/=normTemp; - v[1]/=normTemp; - v[2]/=normTemp; + v *= 1.0 / norm; } } -static inline double TDot3(const double a[3], const double b[3]) -{ - return ((a[0])*(b[0])+(a[1])*(b[1])+(a[2])*(b[2])); -} - -static inline void TCross(const double a[], const double b[], double c[]) -{ - c[0] = (a[1])*(b[2])-(a[2])*(b[1]); - c[1] = (a[2])*(b[0])-(a[0])*(b[2]); - c[2] = (a[0])*(b[1])-(a[1])*(b[0]); -} - -static inline double TAngle3Normalized(const double a[3], const double b[3]) +/** + * \brief Calculate angle between two normalized vectors + * + * \param [in] a normalized vector + * \param [in] b normalized vector + * \return angle between a and b vectors in radians + */ +static inline double TAngle3Normalized(const Vec3d& a, const Vec3d& b) { /* angle = atan2(a dot b, |a x b|) # Bertram (accidental mistake) @@ -91,417 +79,161 @@ static inline double TAngle3Normalized(const double a[3], const double b[3]) angle = acos(a dot b) # Hamdi Sahloul (simplification, a & b are normalized) */ - return acos(TDot3(a, b)); -} - -static inline void matrixProduct33(double *A, double *B, double *R) -{ - R[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6]; - R[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7]; - R[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8]; - - R[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6]; - R[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7]; - R[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8]; - - R[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6]; - R[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7]; - R[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8]; + return acos(a.dot(b)); } -// A is a vector -static inline void matrixProduct133(double *A, double *B, double *R) +static inline void rtToPose(const Matx33d& R, const Vec3d& t, Matx44d& Pose) { - R[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6]; - R[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7]; - R[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8]; + Matx34d P; + hconcat(R, t, P); + vconcat(P, Matx14d(0, 0, 0, 1), Pose); } -static inline void matrixProduct331(const double A[9], const double b[3], double r[3]) +static inline void poseToR(const Matx44d& Pose, Matx33d& R) { - r[0] = A[0] * b[0] + A[1] * b[1] + A[2] * b[2]; - r[1] = A[3] * b[0] + A[4] * b[1] + A[5] * b[2]; - r[2] = A[6] * b[0] + A[7] * b[1] + A[8] * b[2]; + Mat(Pose).rowRange(0, 3).colRange(0, 3).copyTo(R); } -static inline void matrixTranspose33(double *A, double *At) +static inline void poseToRT(const Matx44d& Pose, Matx33d& R, Vec3d& t) { - At[0] = A[0]; - At[4] = A[4]; - At[8] = A[8]; - At[1] = A[3]; - At[2] = A[6]; - At[3] = A[1]; - At[5] = A[7]; - At[6] = A[2]; - At[7] = A[5]; + poseToR(Pose, R); + Mat(Pose).rowRange(0, 3).colRange(3, 4).copyTo(t); } -static inline void matrixProduct44(const double A[16], const double B[16], double R[16]) -{ - R[0] = A[0] * B[0] + A[1] * B[4] + A[2] * B[8] + A[3] * B[12]; - R[1] = A[0] * B[1] + A[1] * B[5] + A[2] * B[9] + A[3] * B[13]; - R[2] = A[0] * B[2] + A[1] * B[6] + A[2] * B[10] + A[3] * B[14]; - R[3] = A[0] * B[3] + A[1] * B[7] + A[2] * B[11] + A[3] * B[15]; - - R[4] = A[4] * B[0] + A[5] * B[4] + A[6] * B[8] + A[7] * B[12]; - R[5] = A[4] * B[1] + A[5] * B[5] + A[6] * B[9] + A[7] * B[13]; - R[6] = A[4] * B[2] + A[5] * B[6] + A[6] * B[10] + A[7] * B[14]; - R[7] = A[4] * B[3] + A[5] * B[7] + A[6] * B[11] + A[7] * B[15]; - - R[8] = A[8] * B[0] + A[9] * B[4] + A[10] * B[8] + A[11] * B[12]; - R[9] = A[8] * B[1] + A[9] * B[5] + A[10] * B[9] + A[11] * B[13]; - R[10] = A[8] * B[2] + A[9] * B[6] + A[10] * B[10] + A[11] * B[14]; - R[11] = A[8] * B[3] + A[9] * B[7] + A[10] * B[11] + A[11] * B[15]; - - R[12] = A[12] * B[0] + A[13] * B[4] + A[14] * B[8] + A[15] * B[12]; - R[13] = A[12] * B[1] + A[13] * B[5] + A[14] * B[9] + A[15] * B[13]; - R[14] = A[12] * B[2] + A[13] * B[6] + A[14] * B[10] + A[15] * B[14]; - R[15] = A[12] * B[3] + A[13] * B[7] + A[14] * B[11] + A[15] * B[15]; -} - -static inline void matrixProduct441(const double A[16], const double B[4], double R[4]) +/** + * \brief Axis angle to rotation + */ +static inline void aaToR(const Vec3d& axis, double angle, Matx33d& R) { - R[0] = A[0] * B[0] + A[1] * B[1] + A[2] * B[2] + A[3] * B[3]; - R[1] = A[4] * B[0] + A[5] * B[1] + A[6] * B[2] + A[7] * B[3]; - R[2] = A[8] * B[0] + A[9] * B[1] + A[10] * B[2] + A[11] * B[3]; - R[3] = A[12] * B[0] + A[13] * B[1] + A[14] * B[2] + A[15] * B[3]; -} + const double sinA = sin(angle); + const double cosA = cos(angle); + const double cos1A = (1 - cosA); + uint i, j; -static inline void matrixPrint(double *A, int m, int n) -{ - int i, j; + Mat(cosA * Matx33d::eye()).copyTo(R); - for (i = 0; i < m; i++) - { - printf(" "); - for (j = 0; j < n; j++) + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) { - printf(" %0.6f ", A[i * n + j]); + if (i != j) + { + // Symmetry skew matrix + R(i, j) += (((i + 1) % 3 == j) ? -1 : 1) * sinA * axis[3 - i - j]; + } + R(i, j) += cos1A * axis[i] * axis[j]; } - printf("\n"); - } -} - -static inline void matrixIdentity(int n, double *A) -{ - int i; - - for (i = 0; i < n*n; i++) - { - A[i] = 0.0; - } - - for (i = 0; i < n; i++) - { - A[i * n + i] = 1.0; - } -} - -static inline void rtToPose(const double R[9], const double t[3], double Pose[16]) -{ - Pose[0]=R[0]; - Pose[1]=R[1]; - Pose[2]=R[2]; - Pose[4]=R[3]; - Pose[5]=R[4]; - Pose[6]=R[5]; - Pose[8]=R[6]; - Pose[9]=R[7]; - Pose[10]=R[8]; - Pose[3]=t[0]; - Pose[7]=t[1]; - Pose[11]=t[2]; - - Pose[15] = 1; -} - - -static inline void poseToRT(const double Pose[16], double R[9], double t[3]) -{ - R[0] = Pose[0]; - R[1] = Pose[1]; - R[2] = Pose[2]; - R[3] = Pose[4]; - R[4] = Pose[5]; - R[5] = Pose[6]; - R[6] = Pose[8]; - R[7] = Pose[9]; - R[8] = Pose[10]; - - t[0]=Pose[3]; - t[1]=Pose[7]; - t[2]=Pose[11]; -} - -static inline void poseToR(const double Pose[16], double R[9]) -{ - R[0] = Pose[0]; - R[1] = Pose[1]; - R[2] = Pose[2]; - R[3] = Pose[4]; - R[4] = Pose[5]; - R[5] = Pose[6]; - R[6] = Pose[8]; - R[7] = Pose[9]; - R[8] = Pose[10]; } /** - * \brief Axis angle to rotation but only compute y and z components + * \brief Compute a rotation in order to rotate around X direction */ -static inline void aaToRyz(double angle, const double r[3], double row2[3], double row3[3]) +static inline void getUnitXRotation(double angle, Matx33d& Rx) { - const double sinA=sin(angle); - const double cosA=cos(angle); - const double cos1A=(1-cosA); - - row2[0] = 0.f; - row2[1] = cosA; - row2[2] = 0.f; - row3[0] = 0.f; - row3[1] = 0.f; - row3[2] = cosA; - - row2[0] += r[2] * sinA; - row2[2] += -r[0] * sinA; - row3[0] += -r[1] * sinA; - row3[1] += r[0] * sinA; - - row2[0] += r[1] * r[0] * cos1A; - row2[1] += r[1] * r[1] * cos1A; - row2[2] += r[1] * r[2] * cos1A; - row3[0] += r[2] * r[0] * cos1A; - row3[1] += r[2] * r[1] * cos1A; - row3[2] += r[2] * r[2] * cos1A; + const double sx = sin(angle); + const double cx = cos(angle); + + Mat(Rx.eye()).copyTo(Rx); + Rx(1, 1) = cx; + Rx(1, 2) = -sx; + Rx(2, 1) = sx; + Rx(2, 2) = cx; } /** - * \brief Axis angle to rotation - */ -static inline void aaToR(double angle, const double r[3], double R[9]) +* \brief Compute a rotation in order to rotate around Y direction +*/ +static inline void getUnitYRotation(double angle, Matx33d& Ry) { - const double sinA=sin(angle); - const double cosA=cos(angle); - const double cos1A=(1-cosA); - double *row1 = &R[0]; - double *row2 = &R[3]; - double *row3 = &R[6]; - - row1[0] = cosA; - row1[1] = 0.0f; - row1[2] = 0.f; - row2[0] = 0.f; - row2[1] = cosA; - row2[2] = 0.f; - row3[0] = 0.f; - row3[1] = 0.f; - row3[2] = cosA; - - row1[1] += -r[2] * sinA; - row1[2] += r[1] * sinA; - row2[0] += r[2] * sinA; - row2[2] += -r[0] * sinA; - row3[0] += -r[1] * sinA; - row3[1] += r[0] * sinA; - - row1[0] += r[0] * r[0] * cos1A; - row1[1] += r[0] * r[1] * cos1A; - row1[2] += r[0] * r[2] * cos1A; - row2[0] += r[1] * r[0] * cos1A; - row2[1] += r[1] * r[1] * cos1A; - row2[2] += r[1] * r[2] * cos1A; - row3[0] += r[2] * r[0] * cos1A; - row3[1] += r[2] * r[1] * cos1A; - row3[2] += r[2] * r[2] * cos1A; + const double sy = sin(angle); + const double cy = cos(angle); + + Mat(Ry.eye()).copyTo(Ry); + Ry(0, 0) = cy; + Ry(0, 2) = sy; + Ry(2, 0) = -sy; + Ry(2, 2) = cy; } /** - * \brief Compute a rotation in order to rotate around X direction - */ -static inline void getUnitXRotation(double angle, double R[9]) +* \brief Compute a rotation in order to rotate around Z direction +*/ +static inline void getUnitZRotation(double angle, Matx33d& Rz) { - const double sinA=sin(angle); - const double cosA=cos(angle); - double *row1 = &R[0]; - double *row2 = &R[3]; - double *row3 = &R[6]; - - row1[0] = 1; - row1[1] = 0.0f; - row1[2] = 0.f; - row2[0] = 0.f; - row2[1] = cosA; - row2[2] = -sinA; - row3[0] = 0.f; - row3[1] = sinA; - row3[2] = cosA; -} -/** - * \brief Compute a transformation in order to rotate around X direction - */ -static inline void getUnitXRotation_44(double angle, double T[16]) -{ - const double sinA=sin(angle); - const double cosA=cos(angle); - double *row1 = &T[0]; - double *row2 = &T[4]; - double *row3 = &T[8]; - - row1[0] = 1; - row1[1] = 0.0f; - row1[2] = 0.f; - row2[0] = 0.f; - row2[1] = cosA; - row2[2] = -sinA; - row3[0] = 0.f; - row3[1] = sinA; - row3[2] = cosA; - - row1[3]=0; - row2[3]=0; - row3[3]=0; - T[3]=0; - T[7]=0; - T[11]=0; - T[15] = 1; + const double sz = sin(angle); + const double cz = cos(angle); + + Mat(Rz.eye()).copyTo(Rz); + Rz(0, 0) = cz; + Rz(0, 1) = -sz; + Rz(1, 0) = sz; + Rz(1, 1) = cz; } /** - * \brief Compute the yz components of the transformation needed to rotate n1 onto x axis and p1 to origin - */ -static inline void computeTransformRTyz(const double p1[4], const double n1[4], double row2[3], double row3[3], double t[3]) +* \brief Convert euler representation to rotation matrix +* +* \param [in] euler RPY angles +* \param [out] R 3x3 Rotation matrix +*/ +static inline void eulerToDCM(const Vec3d& euler, Matx33d& R) { - // dot product with x axis - double angle=acos( n1[0] ); - - // cross product with x axis - double axis[3]={0, n1[2], -n1[1]}; - double axisNorm; - - // we try to project on the ground plane but it's already parallel - if (n1[1]==0 && n1[2]==0) - { - axis[1]=1; - axis[2]=0; - } - else - { - axisNorm=sqrt(axis[2]*axis[2]+axis[1]*axis[1]); - - if (axisNorm>EPS) - { - axis[1]/=axisNorm; - axis[2]/=axisNorm; - } - } + Matx33d Rx, Ry, Rz; - aaToRyz(angle, axis, row2, row3); + getUnitXRotation(euler[0], Rx); + getUnitYRotation(euler[1], Ry); + getUnitZRotation(euler[2], Rz); - t[1] = row2[0] * (-p1[0]) + row2[1] * (-p1[1]) + row2[2] * (-p1[2]); - t[2] = row3[0] * (-p1[0]) + row3[1] * (-p1[1]) + row3[2] * (-p1[2]); + Mat(Rx * (Ry * Rz)).copyTo(R); } + /** * \brief Compute the transformation needed to rotate n1 onto x axis and p1 to origin */ -static inline void computeTransformRT(const double p1[4], const double n1[4], double R[9], double t[3]) +static inline void computeTransformRT(const Vec3d& p1, const Vec3d& n1, Matx33d& R, Vec3d& t) { // dot product with x axis - double angle=acos( n1[0] ); + double angle = acos(n1[0]); // cross product with x axis - double axis[3]={0, n1[2], -n1[1]}; - double axisNorm; - double *row1, *row2, *row3; + Vec3d axis(0, n1[2], -n1[1]); // we try to project on the ground plane but it's already parallel - if (n1[1]==0 && n1[2]==0) + if (n1[1] == 0 && n1[2] == 0) { - axis[1]=1; - axis[2]=0; + axis[1] = 1; + axis[2] = 0; } else { - axisNorm=sqrt(axis[2]*axis[2]+axis[1]*axis[1]); - - if (axisNorm>EPS) - { - axis[1]/=axisNorm; - axis[2]/=axisNorm; - } + TNormalize3(axis); } - aaToR(angle, axis, R); - row1 = &R[0]; - row2 = &R[3]; - row3 = &R[6]; - - t[0] = row1[0] * (-p1[0]) + row1[1] * (-p1[1]) + row1[2] * (-p1[2]); - t[1] = row2[0] * (-p1[0]) + row2[1] * (-p1[1]) + row2[2] * (-p1[2]); - t[2] = row3[0] * (-p1[0]) + row3[1] * (-p1[1]) + row3[2] * (-p1[2]); + aaToR(axis, angle, R); + t = -R * p1; } /** * \brief Flip a normal to the viewing direction * * \param [in] point Scene point - * \param [in] vp_x X component of view direction - * \param [in] vp_y Y component of view direction - * \param [in] vp_z Z component of view direction - * \param [in] nx X component of normal - * \param [in] ny Y component of normal - * \param [in] nz Z component of normal + * \param [in] vp view direction + * \param [in] n normal */ -static inline void flipNormalViewpoint(const float* point, double vp_x, double vp_y, double vp_z, double *nx, double *ny, double *nz) -{ - double cos_theta; - - // See if we need to flip any plane normals - vp_x -= (double)point[0]; - vp_y -= (double)point[1]; - vp_z -= (double)point[2]; - - // Dot product between the (viewpoint - point) and the plane normal - cos_theta = (vp_x * (*nx) + vp_y * (*ny) + vp_z * (*nz)); - - // Flip the plane normal - if (cos_theta < 0) - { - (*nx) *= -1; - (*ny) *= -1; - (*nz) *= -1; - } -} -/** - * \brief Flip a normal to the viewing direction - * - * \param [in] point Scene point - * \param [in] vp_x X component of view direction - * \param [in] vp_y Y component of view direction - * \param [in] vp_z Z component of view direction - * \param [in] nx X component of normal - * \param [in] ny Y component of normal - * \param [in] nz Z component of normal - */ -static inline void flipNormalViewpoint_32f(const float* point, float vp_x, float vp_y, float vp_z, float *nx, float *ny, float *nz) +static inline void flipNormalViewpoint(const Vec3f& point, const Vec3f& vp, Vec3f& n) { float cos_theta; // See if we need to flip any plane normals - vp_x -= (float)point[0]; - vp_y -= (float)point[1]; - vp_z -= (float)point[2]; + Vec3f diff = vp - point; // Dot product between the (viewpoint - point) and the plane normal - cos_theta = (vp_x * (*nx) + vp_y * (*ny) + vp_z * (*nz)); + cos_theta = diff.dot(n); // Flip the plane normal if (cos_theta < 0) { - (*nx) *= -1; - (*ny) *= -1; - (*nz) *= -1; + n *= -1; } } @@ -509,25 +241,16 @@ static inline void flipNormalViewpoint_32f(const float* point, float vp_x, float * \brief Convert a rotation matrix to axis angle representation * * \param [in] R Rotation matrix - * \param [in] axis Axis vector - * \param [in] angle Angle in radians + * \param [out] axis Axis vector + * \param [out] angle Angle in radians */ -static inline void dcmToAA(double *R, double *axis, double *angle) +static inline void dcmToAA(Matx33d& R, Vec3d& axis, double *angle) { - double d1 = R[7] - R[5]; - double d2 = R[2] - R[6]; - double d3 = R[3] - R[1]; - - double norm = sqrt(d1 * d1 + d2 * d2 + d3 * d3); - double x = (R[7] - R[5]) / norm; - double y = (R[2] - R[6]) / norm; - double z = (R[3] - R[1]) / norm; - - *angle = acos((R[0] + R[4] + R[8] - 1.0) * 0.5); - - axis[0] = x; - axis[1] = y; - axis[2] = z; + Mat(Vec3d(R(2, 1) - R(2, 1), + R(0, 2) - R(2, 0), + R(1, 0) - R(0, 1))).copyTo(axis); + TNormalize3(axis); + *angle = acos(0.5 * (cv::trace(R) - 1.0)); } /** @@ -535,39 +258,18 @@ static inline void dcmToAA(double *R, double *axis, double *angle) * * \param [in] axis Axis Vector * \param [in] angle Angle (In radians) - * \param [in] R 3x3 Rotation matrix + * \param [out] R 3x3 Rotation matrix */ -static inline void aaToDCM(double *axis, double angle, double *R) +static inline void aaToDCM(const Vec3d& axis, double angle, Matx33d& R) { - double ident[9]={1,0,0,0,1,0,0,0,1}; - double n[9] = { 0.0, -axis[2], axis[1], - axis[2], 0.0, -axis[0], - -axis[1], axis[0], 0.0 - }; - - double nsq[9]; - double c, s; - int i; - - //c = 1-cos(angle); - c = cos(angle); - s = sin(angle); - - matrixProduct33(n, n, nsq); - - for (i = 0; i < 9; i++) - { - const double sni = n[i]*s; - const double cnsqi = nsq[i]*(c); - R[i]=ident[i]+sni+cnsqi; - } - - // The below code is the matrix based implemntation of the above - // double nsq[9], sn[9], cnsq[9], tmp[9]; - //matrix_scale(3, 3, n, s, sn); - //matrix_scale(3, 3, nsq, (1 - c), cnsq); - //matrix_sum(3, 3, 3, 3, ident, sn, tmp); - //matrix_sum(3, 3, 3, 3, tmp, cnsq, R); + uint i, j; + Matx33d n = Matx33d::all(0); + + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + if (i != j) + n(i, j) = (((i + 1) % 3 == j) ? -1 : 1) * axis[3 - i - j]; + Mat(Matx33d::eye() + sin(angle) * n + cos(angle) * n * n).copyTo(R); } /** @@ -576,52 +278,20 @@ static inline void aaToDCM(double *axis, double angle, double *R) * \param [in] R Rotation Matrix * \param [in] q Quaternion */ -static inline void dcmToQuat(double *R, double *q) +static inline void dcmToQuat(Matx33d& R, Vec4d& q) { - double n4; // the norm of quaternion multiplied by 4 - double tr = R[0] + R[4] + R[8]; // trace of martix - double factor; - - if (tr > 0.0) + double tr = cv::trace(R); + Vec3d v(R(0, 0), R(1, 1), R(2, 2)); + int idx = tr > 0.0 ? 3 : (int)(std::max_element(v.val, v.val + 3) - v.val); + double norm4 = q[(idx + 1) % 4] = 1.0 + (tr > 0.0 ? tr : 2 * R(idx, idx) - tr); + int i, prev, next, step = idx % 2 ? 1 : -1, curr = 3; + for (i = 0; i < 3; i++) { - q[1] = R[5] - R[7]; - q[2] = R[6] - R[2]; - q[3] = R[1] - R[3]; - q[0] = tr + 1.0; - n4 = q[0]; + curr = (curr + step) % 4; + next = (curr + 1) % 3, prev = (curr + 2) % 3; + q[(idx + i + 2) % 4] = R(next, prev) + (tr > 0.0 || idx == curr ? -1 : 1) * R(prev, next); } - else - if ((R[0] > R[4]) && (R[0] > R[8])) - { - q[1] = 1.0 + R[0] - R[4] - R[8]; - q[2] = R[3] + R[1]; - q[3] = R[6] + R[2]; - q[0] = R[5] - R[7]; - n4 = q[1]; - } - else - if (R[4] > R[8]) - { - q[1] = R[3] + R[1]; - q[2] = 1.0 + R[4] - R[0] - R[8]; - q[3] = R[7] + R[5]; - q[0] = R[6] - R[2]; - n4 = q[2]; - } - else - { - q[1] = R[6] + R[2]; - q[2] = R[7] + R[5]; - q[3] = 1.0 + R[8] - R[0] - R[4]; - q[0] = R[1] - R[3]; - n4 = q[3]; - } - - factor = 0.5 / sqrt(n4); - q[0] *= factor; - q[1] *= factor; - q[2] *= factor; - q[3] *= factor; + q *= 0.5 / sqrt(norm4); } /** @@ -631,36 +301,33 @@ static inline void dcmToQuat(double *R, double *q) * \param [in] R Rotation Matrix * */ -static inline void quatToDCM(double *q, double *R) +static inline void quatToDCM(Vec4d& q, Matx33d& R) { - double sqw = q[0] * q[0]; - double sqx = q[1] * q[1]; - double sqy = q[2] * q[2]; - double sqz = q[3] * q[3]; + Vec4d sq = q.mul(q); double tmp1, tmp2; - R[0] = sqx - sqy - sqz + sqw; // since sqw + sqx + sqy + sqz = 1 - R[4] = -sqx + sqy - sqz + sqw; - R[8] = -sqx - sqy + sqz + sqw; + R(0, 0) = sq[0] + sq[1] - sq[2] - sq[3]; // since norm(q) = 1 + R(1, 1) = sq[0] - sq[1] + sq[2] - sq[3]; + R(2, 2) = sq[0] - sq[1] - sq[2] + sq[3]; tmp1 = q[1] * q[2]; tmp2 = q[3] * q[0]; - R[1] = 2.0 * (tmp1 + tmp2); - R[3] = 2.0 * (tmp1 - tmp2); + R(0, 1) = 2.0 * (tmp1 + tmp2); + R(1, 0) = 2.0 * (tmp1 - tmp2); tmp1 = q[1] * q[3]; tmp2 = q[2] * q[0]; - R[2] = 2.0 * (tmp1 - tmp2); - R[6] = 2.0 * (tmp1 + tmp2); + R(0, 2) = 2.0 * (tmp1 - tmp2); + R(2, 0) = 2.0 * (tmp1 + tmp2); tmp1 = q[2] * q[3]; tmp2 = q[1] * q[0]; - R[5] = 2.0 * (tmp1 + tmp2); - R[7] = 2.0 * (tmp1 - tmp2); + R(1, 2) = 2.0 * (tmp1 + tmp2); + R(2, 1) = 2.0 * (tmp1 - tmp2); } } // namespace ppf_match_3d diff --git a/modules/surface_matching/src/hash_murmur64.hpp b/modules/surface_matching/src/hash_murmur64.hpp index 75eddb591..1cf544c1a 100644 --- a/modules/surface_matching/src/hash_murmur64.hpp +++ b/modules/surface_matching/src/hash_murmur64.hpp @@ -26,7 +26,7 @@ THE SOFTWARE. // Block read - if your platform needs to do endian-swapping or can only // handle aligned reads, do the conversion here -FORCE_INLINE unsigned int getblock ( const unsigned int * p, int i ) +FORCE_INLINE uint getblock ( const uint * p, int i ) { return p[i]; } @@ -36,7 +36,7 @@ FORCE_INLINE unsigned int getblock ( const unsigned int * p, int i ) // avalanches all bits to within 0.25% bias -FORCE_INLINE unsigned int fmix32 ( unsigned int h ) +FORCE_INLINE uint fmix32 ( uint h ) { h ^= h >> 16; h *= 0x85ebca6b; @@ -49,7 +49,7 @@ FORCE_INLINE unsigned int fmix32 ( unsigned int h ) //----------------------------------------------------------------------------- -FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & k1, unsigned int & c1, unsigned int & c2 ) +FORCE_INLINE void bmix32 ( uint & h1, uint & k1, uint & c1, uint & c2 ) { k1 *= c1; k1 = ROTL32(k1,11); @@ -64,7 +64,7 @@ FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & k1, unsigned int & //----------------------------------------------------------------------------- -FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & h2, unsigned int & k1, unsigned int & k2, unsigned int & c1, unsigned int & c2 ) +FORCE_INLINE void bmix32 ( uint & h1, uint & h2, uint & k1, uint & k2, uint & c1, uint & c2 ) { k1 *= c1; k1 = ROTL32(k1,11); @@ -89,26 +89,26 @@ FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & h2, unsigned int & //---------- -FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigned int seed, void * out ) +FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const uint seed, void * out ) { - const unsigned char * data = (const unsigned char*)key; + const uchar * data = (const uchar*)key; const int nblocks = len / 8; - unsigned int h1 = 0x8de1c3ac ^ seed; - unsigned int h2 = 0xbab98226 ^ seed; + uint h1 = 0x8de1c3ac ^ seed; + uint h2 = 0xbab98226 ^ seed; - unsigned int c1 = 0x95543787; - unsigned int c2 = 0x2ad7eb25; + uint c1 = 0x95543787; + uint c2 = 0x2ad7eb25; //---------- // body - const unsigned int * blocks = (const unsigned int *)(data + nblocks*8); + const uint * blocks = (const uint *)(data + nblocks*8); for (int i = -nblocks; i; i++) { - unsigned int k1 = getblock(blocks,i*2+0); - unsigned int k2 = getblock(blocks,i*2+1); + uint k1 = getblock(blocks,i*2+0); + uint k2 = getblock(blocks,i*2+1); bmix32(h1,h2,k1,k2,c1,c2); } @@ -116,10 +116,10 @@ FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigne //---------- // tail - const unsigned char * tail = (const unsigned char*)(data + nblocks*8); + const uchar * tail = (const uchar*)(data + nblocks*8); - unsigned int k1 = 0; - unsigned int k2 = 0; + uint k1 = 0; + uint k2 = 0; switch (len & 7) { @@ -154,8 +154,8 @@ FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigne h1 += h2; h2 += h1; - ((unsigned int*)out)[0] = h1; - ((unsigned int*)out)[1] = h2; + ((uint*)out)[0] = h1; + ((uint*)out)[1] = h2; } diff --git a/modules/surface_matching/src/hash_murmur86.hpp b/modules/surface_matching/src/hash_murmur86.hpp index 85a61138d..7b7d01524 100644 --- a/modules/surface_matching/src/hash_murmur86.hpp +++ b/modules/surface_matching/src/hash_murmur86.hpp @@ -14,9 +14,13 @@ /* We can't use the name 'uint32_t' here because it will conflict with * any version provided by the system headers or application. */ +#if !defined(ulong) +#define ulong unsigned long +#endif + /* First look for special cases */ #if defined(_MSC_VER) -#define MH_UINT32 unsigned long +#define MH_UINT32 ulong #endif /* If the compiler says it's C99 then take its word for it */ @@ -30,25 +34,25 @@ #if !defined(MH_UINT32) #include #if (USHRT_MAX == 0xffffffffUL) -#define MH_UINT32 unsigned short +#define MH_UINT32 ushort #elif (UINT_MAX == 0xffffffffUL) -#define MH_UINT32 unsigned int +#define MH_UINT32 uint #elif (ULONG_MAX == 0xffffffffUL) -#define MH_UINT32 unsigned long +#define MH_UINT32 ulong #endif #endif #if !defined(MH_UINT32) -#error Unable to determine type name for unsigned 32-bit int +#error Unable to determine type name for u32-bit int #endif -/* I'm yet to work on a platform where 'unsigned char' is not 8 bits */ -#define MH_UINT8 unsigned char +/* I'm yet to work on a platform where 'uchar' is not 8 bits */ +#define MH_UINT8 uchar void PMurHash32_Process(MH_UINT32 *ph1, MH_UINT32 *pcarry, const void *key, int len); MH_UINT32 PMurHash32_Result(MH_UINT32 h1, MH_UINT32 carry, MH_UINT32 total_length); MH_UINT32 PMurHash32(MH_UINT32 seed, const void *key, int len); -void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, void * out ); +void hashMurmurx86 ( const void * key, const int len, const uint seed, void * out ); /* I used ugly type names in the header to avoid potential conflicts with * application or system typedefs & defines. Since I'm not including any more @@ -69,7 +73,7 @@ void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, v * The following 3 macros are defined in this section. The other macros defined * are only needed to help derive these 3. * -* READ_UINT32(x) Read a little endian unsigned 32-bit int +* READ_UINT32(x) Read a little endian u32-bit int * UNALIGNED_SAFE Defined if READ_UINT32 works on non-word boundaries * ROTL32(x,r) Rotate x left by r bits */ @@ -169,23 +173,23 @@ void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int le { uint32_t h1 = *ph1; uint32_t c = *pcarry; - + const uint8_t *ptr = (uint8_t*) key; const uint8_t *end; - + /* Extract carry count from low 2 bits of c value */ int n = c & 3; - + #if defined(UNALIGNED_SAFE) /* This CPU handles unaligned word access */ - + /* Consume any carry bytes */ int i = (4-n) & 3; if (i && i <= len) { DOBYTES(i, h1, c, n, ptr, len); } - + /* Process 32-bit chunks */ end = ptr + len/4*4; for ( ; ptr < end ; ptr+=4) @@ -193,17 +197,17 @@ void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int le uint32_t k1 = READ_UINT32(ptr); DOBLOCK(h1, k1); } - + #else /*UNALIGNED_SAFE*/ /* This CPU does not handle unaligned word access */ - + /* Consume enough so that the next data byte is word aligned */ int i = -(long)ptr & 3; if (i && i <= len) { DOBYTES(i, h1, c, n, ptr, len); } - + /* We're now aligned. Process in aligned blocks. Specialise for each possible carry count */ end = ptr + len/4*4; switch (n) @@ -244,13 +248,13 @@ void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int le } } #endif /*UNALIGNED_SAFE*/ - + /* Advance over whole 32-bit chunks, possibly leaving 1..3 bytes */ len -= len/4*4; - + /* Append any remaining bytes into carry */ DOBYTES(len, h1, c, n, ptr, len); - + /* Copy out new running hash and carry */ *ph1 = h1; *pcarry = (c & ~0xff) | n; @@ -272,14 +276,14 @@ uint32_t PMurHash32_Result(uint32_t h, uint32_t carry, uint32_t total_length) h ^= k1; } h ^= total_length; - + /* fmix */ h ^= h >> 16; h *= 0x85ebca6b; h ^= h >> 13; h *= 0xc2b2ae35; h ^= h >> 16; - + return h; } @@ -293,8 +297,8 @@ uint32_t PMurHash32(uint32_t seed, const void *key, int len) return PMurHash32_Result(h1, carry, len); } -void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, void * out ) +void hashMurmurx86 ( const void * key, const int len, const uint seed, void * out ) { - *(unsigned int*)out = PMurHash32 (seed, key, len); + *(uint*)out = PMurHash32 (seed, key, len); } diff --git a/modules/surface_matching/src/icp.cpp b/modules/surface_matching/src/icp.cpp index 805cc0084..a6f0822cf 100644 --- a/modules/surface_matching/src/icp.cpp +++ b/modules/surface_matching/src/icp.cpp @@ -44,7 +44,7 @@ namespace cv { namespace ppf_match_3d { -static void subtractColumns(Mat srcPC, double mean[3]) +static void subtractColumns(Mat srcPC, Vec3d& mean) { int height = srcPC.rows; @@ -60,7 +60,7 @@ static void subtractColumns(Mat srcPC, double mean[3]) } // as in PCA -static void computeMeanCols(Mat srcPC, double mean[3]) +static void computeMeanCols(Mat srcPC, Vec3d& mean) { int height = srcPC.rows; @@ -86,7 +86,7 @@ static void computeMeanCols(Mat srcPC, double mean[3]) } // as in PCA -/*static void subtractMeanFromColumns(Mat srcPC, double mean[3]) +/*static void subtractMeanFromColumns(Mat srcPC, Vec3d& mean) { computeMeanCols(srcPC, mean); subtractColumns(srcPC, mean); @@ -192,88 +192,38 @@ static float getRejectionThreshold(float* r, int m, float outlierScale) } // Kok Lim Low's linearization -static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Mat& X) +static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Vec3d& rpy, Vec3d& t) { //Mat sub = Dst - Src; Mat A = Mat(Src.rows, 6, CV_64F); Mat b = Mat(Src.rows, 1, CV_64F); + Mat rpy_t; #if defined _OPENMP #pragma omp parallel for #endif for (int i=0; i(i); - const double *dstPt = Dst.ptr(i); - const double *normals = &dstPt[3]; - double *bVal = b.ptr(i); - double *aRow = A.ptr(i); - - const double sub[3]={dstPt[0]-srcPt[0], dstPt[1]-srcPt[1], dstPt[2]-srcPt[2]}; - - *bVal = TDot3(sub, normals); - TCross(srcPt, normals, aRow); - - aRow[3] = normals[0]; - aRow[4] = normals[1]; - aRow[5] = normals[2]; + const Vec3d srcPt(Src.ptr(i)); + const Vec3d dstPt(Dst.ptr(i)); + const Vec3d normals(Dst.ptr(i) + 3); + const Vec3d sub = dstPt - srcPt; + const Vec3d axis = srcPt.cross(normals); + + *b.ptr(i) = sub.dot(normals); + hconcat(axis.reshape<1, 3>(), normals.reshape<1, 3>(), A.row(i)); } - cv::solve(A, b, X, DECOMP_SVD); + cv::solve(A, b, rpy_t, DECOMP_SVD); + rpy_t.rowRange(0, 3).copyTo(rpy); + rpy_t.rowRange(3, 6).copyTo(t); } - -static void getTransformMat(Mat X, double Pose[16]) +static void getTransformMat(Vec3d& euler, Vec3d& t, Matx44d& Pose) { - Mat DCM; - double *r1, *r2, *r3; - double* x = (double*)X.data; - - const double sx = sin(x[0]); - const double cx = cos(x[0]); - const double sy = sin(x[1]); - const double cy = cos(x[1]); - const double sz = sin(x[2]); - const double cz = cos(x[2]); - - Mat R1 = Mat::eye(3,3, CV_64F); - Mat R2 = Mat::eye(3,3, CV_64F); - Mat R3 = Mat::eye(3,3, CV_64F); - - r1= (double*)R1.data; - r2= (double*)R2.data; - r3= (double*)R3.data; - - r1[4]= cx; - r1[5]= -sx; - r1[7]= sx; - r1[8]= cx; - - r2[0]= cy; - r2[2]= sy; - r2[6]= -sy; - r2[8]= cy; - - r3[0]= cz; - r3[1]= -sz; - r3[3]= sz; - r3[4]= cz; - - DCM = R1*(R2*R3); - - Pose[0] = DCM.at(0,0); - Pose[1] = DCM.at(0,1); - Pose[2] = DCM.at(0,2); - Pose[4] = DCM.at(1,0); - Pose[5] = DCM.at(1,1); - Pose[6] = DCM.at(1,2); - Pose[8] = DCM.at(2,0); - Pose[9] = DCM.at(2,1); - Pose[10] = DCM.at(2,2); - Pose[3]=x[3]; - Pose[7]=x[4]; - Pose[11]=x[5]; - Pose[15]=1; + Matx33d R; + eulerToDCM(euler, R); + rtToPose(R, t, Pose); } /* Fast way to look up the duplicates @@ -301,10 +251,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu Mat srcTemp = srcPC.clone(); Mat dstTemp = dstPC.clone(); - double meanSrc[3], meanDst[3]; + Vec3d meanSrc, meanDst; computeMeanCols(srcTemp, meanSrc); computeMeanCols(dstTemp, meanDst); - double meanAvg[3]={0.5*(meanSrc[0]+meanDst[0]), 0.5*(meanSrc[1]+meanDst[1]), 0.5*(meanSrc[2]+meanDst[2])}; + Vec3d meanAvg = 0.5 * (meanSrc + meanDst); subtractColumns(srcTemp, meanAvg); subtractColumns(dstTemp, meanAvg); @@ -320,7 +270,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu Mat dstPC0 = dstTemp; // initialize pose - matrixIdentity(4, pose.val); + pose = Matx44d::eye(); Mat M = Mat::eye(4,4,CV_64F); @@ -338,7 +288,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1)); // Obtain the sampled point clouds for this level: Also rotates the normals - Mat srcPCT = transformPCPose(srcPC0, pose.val); + Mat srcPCT = transformPCPose(srcPC0, pose); const int sampleStep = cvRound((double)n/(double)numSamples); @@ -372,8 +322,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu int* newI = new int[numElSrc]; int* newJ = new int[numElSrc]; - double PoseX[16]={0}; - matrixIdentity(4, PoseX); + Matx44d PoseX = Matx44d::eye(); while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && ipose); registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP); - poses[i]->appendPose(poseICP.val); + poses[i]->appendPose(poseICP); } return 0; } diff --git a/modules/surface_matching/src/pose_3d.cpp b/modules/surface_matching/src/pose_3d.cpp index 808cb9033..165ec1974 100644 --- a/modules/surface_matching/src/pose_3d.cpp +++ b/modules/surface_matching/src/pose_3d.cpp @@ -45,29 +45,15 @@ namespace cv namespace ppf_match_3d { -void Pose3D::updatePose(double NewPose[16]) +void Pose3D::updatePose(Matx44d& NewPose) { - double R[9]; + Matx33d R; - for (int i=0; i<16; i++) - pose[i]=NewPose[i]; - - R[0] = pose[0]; - R[1] = pose[1]; - R[2] = pose[2]; - R[3] = pose[4]; - R[4] = pose[5]; - R[5] = pose[6]; - R[6] = pose[8]; - R[7] = pose[9]; - R[8] = pose[10]; - - t[0]=pose[3]; - t[1]=pose[7]; - t[2]=pose[11]; + pose = NewPose; + poseToRT(pose, R, t); // compute the angle - const double trace = R[0] + R[4] + R[8]; + const double trace = cv::trace(R); if (fabs(trace - 3) <= EPS) { @@ -87,27 +73,12 @@ void Pose3D::updatePose(double NewPose[16]) dcmToQuat(R, q); } -void Pose3D::updatePose(double NewR[9], double NewT[3]) +void Pose3D::updatePose(Matx33d& NewR, Vec3d& NewT) { - pose[0]=NewR[0]; - pose[1]=NewR[1]; - pose[2]=NewR[2]; - pose[3]=NewT[0]; - pose[4]=NewR[3]; - pose[5]=NewR[4]; - pose[6]=NewR[5]; - pose[7]=NewT[1]; - pose[8]=NewR[6]; - pose[9]=NewR[7]; - pose[10]=NewR[8]; - pose[11]=NewT[2]; - pose[12]=0; - pose[13]=0; - pose[14]=0; - pose[15]=1; + rtToPose(NewR, NewT, pose); // compute the angle - const double trace = NewR[0] + NewR[4] + NewR[8]; + const double trace = cv::trace(NewR); if (fabs(trace - 3) <= EPS) { @@ -127,35 +98,17 @@ void Pose3D::updatePose(double NewR[9], double NewT[3]) dcmToQuat(NewR, q); } -void Pose3D::updatePoseQuat(double Q[4], double NewT[3]) +void Pose3D::updatePoseQuat(Vec4d& Q, Vec3d& NewT) { - double NewR[9]; + Matx33d NewR; quatToDCM(Q, NewR); - q[0]=Q[0]; - q[1]=Q[1]; - q[2]=Q[2]; - q[3]=Q[3]; - - pose[0]=NewR[0]; - pose[1]=NewR[1]; - pose[2]=NewR[2]; - pose[3]=NewT[0]; - pose[4]=NewR[3]; - pose[5]=NewR[4]; - pose[6]=NewR[5]; - pose[7]=NewT[1]; - pose[8]=NewR[6]; - pose[9]=NewR[7]; - pose[10]=NewR[8]; - pose[11]=NewT[2]; - pose[12]=0; - pose[13]=0; - pose[14]=0; - pose[15]=1; + q = Q; + + rtToPose(NewR, NewT, pose); // compute the angle - const double trace = NewR[0] + NewR[4] + NewR[8]; + const double trace = cv::trace(NewR); if (fabs(trace - 3) <= EPS) { @@ -175,28 +128,15 @@ void Pose3D::updatePoseQuat(double Q[4], double NewT[3]) } -void Pose3D::appendPose(double IncrementalPose[16]) +void Pose3D::appendPose(Matx44d& IncrementalPose) { - double R[9], PoseFull[16]={0}; + Matx33d R; + Matx44d PoseFull = IncrementalPose * this->pose; - matrixProduct44(IncrementalPose, this->pose, PoseFull); - - R[0] = PoseFull[0]; - R[1] = PoseFull[1]; - R[2] = PoseFull[2]; - R[3] = PoseFull[4]; - R[4] = PoseFull[5]; - R[5] = PoseFull[6]; - R[6] = PoseFull[8]; - R[7] = PoseFull[9]; - R[8] = PoseFull[10]; - - t[0]=PoseFull[3]; - t[1]=PoseFull[7]; - t[2]=PoseFull[11]; + poseToRT(PoseFull, R, t); // compute the angle - const double trace = R[0] + R[4] + R[8]; + const double trace = cv::trace(R); if (fabs(trace - 3) <= EPS) { @@ -215,42 +155,25 @@ void Pose3D::appendPose(double IncrementalPose[16]) // compute the quaternion dcmToQuat(R, q); - for (int i=0; i<16; i++) - pose[i]=PoseFull[i]; + pose = PoseFull; } Pose3DPtr Pose3D::clone() { Ptr new_pose(new Pose3D(alpha, modelIndex, numVotes)); - for (int i=0; i<16; i++) - new_pose->pose[i]= this->pose[i]; - - new_pose->q[0]=q[0]; - new_pose->q[1]=q[1]; - new_pose->q[2]=q[2]; - new_pose->q[3]=q[3]; - new_pose->t[0]=t[0]; - new_pose->t[1]=t[1]; - new_pose->t[2]=t[2]; - - new_pose->angle=angle; + new_pose->pose = this->pose; + new_pose->q = q; + new_pose->t = t; + new_pose->angle = angle; return new_pose; } void Pose3D::printPose() { - printf("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n", this->modelIndex, this->numVotes, this->residual); - for (int j=0; j<4; j++) - { - for (int k=0; k<4; k++) - { - printf("%f ", this->pose[j*4+k]); - } - printf("\n"); - } - printf("\n"); + printf("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n", (uint)this->modelIndex, (uint)this->numVotes, this->residual); + std::cout << this->pose << std::endl; } int Pose3D::writePose(FILE* f) @@ -260,9 +183,9 @@ int Pose3D::writePose(FILE* f) fwrite(&angle, sizeof(double), 1, f); fwrite(&numVotes, sizeof(int), 1, f); fwrite(&modelIndex, sizeof(int), 1, f); - fwrite(pose, sizeof(double)*16, 1, f); - fwrite(t, sizeof(double)*3, 1, f); - fwrite(q, sizeof(double)*4, 1, f); + fwrite(pose.val, sizeof(double)*16, 1, f); + fwrite(t.val, sizeof(double)*3, 1, f); + fwrite(q.val, sizeof(double)*4, 1, f); fwrite(&residual, sizeof(double), 1, f); return 0; } @@ -277,9 +200,9 @@ int Pose3D::readPose(FILE* f) status = fread(&angle, sizeof(double), 1, f); status = fread(&numVotes, sizeof(int), 1, f); status = fread(&modelIndex, sizeof(int), 1, f); - status = fread(pose, sizeof(double)*16, 1, f); - status = fread(t, sizeof(double)*3, 1, f); - status = fread(q, sizeof(double)*4, 1, f); + status = fread(pose.val, sizeof(double)*16, 1, f); + status = fread(t.val, sizeof(double)*3, 1, f); + status = fread(q.val, sizeof(double)*4, 1, f); status = fread(&residual, sizeof(double), 1, f); return 0; } diff --git a/modules/surface_matching/src/ppf_helpers.cpp b/modules/surface_matching/src/ppf_helpers.cpp index f652794f5..fb59f9a2e 100644 --- a/modules/surface_matching/src/ppf_helpers.cpp +++ b/modules/surface_matching/src/ppf_helpers.cpp @@ -50,10 +50,10 @@ typedef cv::flann::GenericIndex< Distance_32F > FlannIndex; void shuffle(int *array, size_t n); Mat genRandomMat(int rows, int cols, double mean, double stddev, int type); -void getRandQuat(double q[4]); -void getRandomRotation(double R[9]); -void meanCovLocalPC(const float* pc, const int ws, const int point_count, double CovMat[3][3], double Mean[4]); -void meanCovLocalPCInd(const float* pc, const int* Indices, const int ws, const int point_count, double CovMat[3][3], double Mean[4]); +void getRandQuat(Vec4d& q); +void getRandomRotation(Matx33d& R); +void meanCovLocalPC(const Mat& pc, const int point_count, Matx33d& CovMat, Vec3d& Mean); +void meanCovLocalPCInd(const Mat& pc, const int* Indices, const int point_count, Matx33d& CovMat, Vec3d& Mean); static std::vector split(const std::string &text, char sep) { std::vector tokens; @@ -183,7 +183,7 @@ void writePLY(Mat PC, const char* FileName) { const float* point = PC.ptr(pi); - outFile << point[0] << " "< > map; @@ -466,7 +466,7 @@ void shuffle(int *array, size_t n) } // compute the standard bounding box -void computeBboxStd(Mat pc, float xRange[2], float yRange[2], float zRange[2]) +void computeBboxStd(Mat pc, Vec2f& xRange, Vec2f& yRange, Vec2f& zRange) { Mat pcPts = pc.colRange(0, 3); int num = pcPts.rows; @@ -513,9 +513,9 @@ Mat normalizePCCoeff(Mat pc, float scale, float* Cx, float* Cy, float* Cz, float pc.col(1).copyTo(y); pc.col(2).copyTo(z); - float cx = (float) cv::mean(x).val[0]; - float cy = (float) cv::mean(y).val[0]; - float cz = (float) cv::mean(z).val[0]; + float cx = (float) cv::mean(x)[0]; + float cy = (float) cv::mean(y)[0]; + float cz = (float) cv::mean(z)[0]; cv::minMaxIdx(pc, &minVal, &maxVal); @@ -559,11 +559,12 @@ Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal return pcn; } -Mat transformPCPose(Mat pc, const double Pose[16]) +Mat transformPCPose(Mat pc, const Matx44d& Pose) { Mat pct = Mat(pc.rows, pc.cols, CV_32F); - double R[9], t[3]; + Matx33d R; + Vec3d t; poseToRT(Pose, R, t); #if defined _OPENMP @@ -572,37 +573,29 @@ Mat transformPCPose(Mat pc, const double Pose[16]) for (int i=0; i(i); - float *pcDataT = pct.ptr(i); - const float *n1 = &pcData[3]; - float *nT = &pcDataT[3]; - - double p[4] = {(double)pcData[0], (double)pcData[1], (double)pcData[2], 1}; - double p2[4]; + const Vec3f n1(&pcData[3]); - matrixProduct441(Pose, p, p2); + Vec4d p = Pose * Vec4d(pcData[0], pcData[1], pcData[2], 1); + Vec3d p2(p.val); // p2[3] should normally be 1 - if (fabs(p2[3])>EPS) + if (fabs(p[3]) > EPS) { - pcDataT[0] = (float)(p2[0]/p2[3]); - pcDataT[1] = (float)(p2[1]/p2[3]); - pcDataT[2] = (float)(p2[2]/p2[3]); + Mat((1.0 / p[3]) * p2).reshape(1, 1).convertTo(pct.row(i).colRange(0, 3), CV_32F); } // If the point cloud has normals, // then rotate them as well if (pc.cols == 6) { - double n[3] = { (double)n1[0], (double)n1[1], (double)n1[2] }, n2[3]; + Vec3d n(n1), n2; - matrixProduct331(R, n, n2); - double nNorm = sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]); + n2 = R * n; + double nNorm = cv::norm(n2); - if (nNorm>EPS) + if (nNorm > EPS) { - nT[0]=(float)(n2[0]/nNorm); - nT[1]=(float)(n2[1]/nNorm); - nT[2]=(float)(n2[2]/nNorm); + Mat((1.0 / nNorm) * n2).reshape(1, 1).convertTo(pct.row(i).colRange(3, 6), CV_32F); } } } @@ -621,32 +614,28 @@ Mat genRandomMat(int rows, int cols, double mean, double stddev, int type) return matr; } -void getRandQuat(double q[4]) +void getRandQuat(Vec4d& q) { q[0] = (float)rand()/(float)(RAND_MAX); q[1] = (float)rand()/(float)(RAND_MAX); q[2] = (float)rand()/(float)(RAND_MAX); q[3] = (float)rand()/(float)(RAND_MAX); - double n = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]); - q[0]/=n; - q[1]/=n; - q[2]/=n; - q[3]/=n; - + q *= 1.0 / cv::norm(q); q[0]=fabs(q[0]); } -void getRandomRotation(double R[9]) +void getRandomRotation(Matx33d& R) { - double q[4]; + Vec4d q; getRandQuat(q); quatToDCM(q, R); } -void getRandomPose(double Pose[16]) +void getRandomPose(Matx44d& Pose) { - double R[9], t[3]; + Matx33d R; + Vec3d t; srand((unsigned int)time(0)); getRandomRotation(R); @@ -672,84 +661,37 @@ to improve accuracy and increase speed Also, view point flipping as in point cloud library is implemented */ -void meanCovLocalPC(const float* pc, const int ws, const int point_count, double CovMat[3][3], double Mean[4]) +void meanCovLocalPC(const Mat& pc, const int point_count, Matx33d& CovMat, Vec3d& Mean) { - int i; - double accu[16]={0}; - - // For each point in the cloud - for (i = 0; i < point_count; ++i) - { - const float* cloud = &pc[i*ws]; - accu [0] += cloud[0] * cloud[0]; - accu [1] += cloud[0] * cloud[1]; - accu [2] += cloud[0] * cloud[2]; - accu [3] += cloud[1] * cloud[1]; // 4 - accu [4] += cloud[1] * cloud[2]; // 5 - accu [5] += cloud[2] * cloud[2]; // 8 - accu [6] += cloud[0]; - accu [7] += cloud[1]; - accu [8] += cloud[2]; - } - - for (i = 0; i < 9; ++i) - accu[i]/=(double)point_count; - - Mean[0] = accu[6]; - Mean[1] = accu[7]; - Mean[2] = accu[8]; - Mean[3] = 0; - CovMat[0][0] = accu [0] - accu [6] * accu [6]; - CovMat[0][1] = accu [1] - accu [6] * accu [7]; - CovMat[0][2] = accu [2] - accu [6] * accu [8]; - CovMat[1][1] = accu [3] - accu [7] * accu [7]; - CovMat[1][2] = accu [4] - accu [7] * accu [8]; - CovMat[2][2] = accu [5] - accu [8] * accu [8]; - CovMat[1][0] = CovMat[0][1]; - CovMat[2][0] = CovMat[0][2]; - CovMat[2][1] = CovMat[1][2]; - + cv::calcCovarMatrix(pc.rowRange(0, point_count), CovMat, Mean, cv::COVAR_NORMAL | cv::COVAR_ROWS); + CovMat *= 1.0 / (point_count - 1); } -void meanCovLocalPCInd(const float* pc, const int* Indices, const int ws, const int point_count, double CovMat[3][3], double Mean[4]) +void meanCovLocalPCInd(const Mat& pc, const int* Indices, const int point_count, Matx33d& CovMat, Vec3d& Mean) { - int i; - double accu[16]={0}; + int i, j, k; + CovMat = Matx33d::all(0); + Mean = Vec3d::all(0); for (i = 0; i < point_count; ++i) { - const float* cloud = &pc[ Indices[i] * ws ]; - accu [0] += cloud[0] * cloud[0]; - accu [1] += cloud[0] * cloud[1]; - accu [2] += cloud[0] * cloud[2]; - accu [3] += cloud[1] * cloud[1]; // 4 - accu [4] += cloud[1] * cloud[2]; // 5 - accu [5] += cloud[2] * cloud[2]; // 8 - accu [6] += cloud[0]; - accu [7] += cloud[1]; - accu [8] += cloud[2]; + const float* cloud = pc.ptr(Indices[i]); + for (j = 0; j < 3; ++j) + { + for (k = 0; k < 3; ++k) + CovMat(j, k) += cloud[j] * cloud[k]; + Mean[j] += cloud[j]; + } } + Mean *= 1.0 / point_count; + CovMat *= 1.0 / point_count; - for (i = 0; i < 9; ++i) - accu[i]/=(double)point_count; - - Mean[0] = accu[6]; - Mean[1] = accu[7]; - Mean[2] = accu[8]; - Mean[3] = 0; - CovMat[0][0] = accu [0] - accu [6] * accu [6]; - CovMat[0][1] = accu [1] - accu [6] * accu [7]; - CovMat[0][2] = accu [2] - accu [6] * accu [8]; - CovMat[1][1] = accu [3] - accu [7] * accu [7]; - CovMat[1][2] = accu [4] - accu [7] * accu [8]; - CovMat[2][2] = accu [5] - accu [8] * accu [8]; - CovMat[1][0] = CovMat[0][1]; - CovMat[2][0] = CovMat[0][2]; - CovMat[2][1] = CovMat[1][2]; - + for (j = 0; j < 3; ++j) + for (k = 0; k < 3; ++k) + CovMat(j, k) -= Mean[j] * Mean[k]; } -CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3d& viewpoint) +int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3f& viewpoint) { int i; @@ -759,86 +701,45 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe CV_Error(cv::Error::BadImageSize, "PC should have 3 or 6 elements in its columns"); } - int sizes[2] = {PC.rows, 3}; - int sizesResult[2] = {PC.rows, NumNeighbors}; - float* dataset = new float[PC.rows*3]; - float* distances = new float[PC.rows*NumNeighbors]; - int* indices = new int[PC.rows*NumNeighbors]; + PCNormals.create(PC.rows, 6, CV_32F); + Mat PCInput = PCNormals.colRange(0, 3); + Mat Distances(PC.rows, NumNeighbors, CV_32F); + Mat Indices(PC.rows, NumNeighbors, CV_32S); - for (i=0; i(i); - float* dst = (float*)(&dataset[i*3]); - - dst[0] = src[0]; - dst[1] = src[1]; - dst[2] = src[2]; - } - - Mat PCInput(2, sizes, CV_32F, dataset, 0); + PC.rowRange(0, PC.rows).colRange(0, 3).copyTo(PCNormals.rowRange(0, PC.rows).colRange(0, 3)); void* flannIndex = indexPCFlann(PCInput); - Mat Indices(2, sizesResult, CV_32S, indices, 0); - Mat Distances(2, sizesResult, CV_32F, distances, 0); - queryPCFlann(flannIndex, PCInput, Indices, Distances, NumNeighbors); destroyFlann(flannIndex); flannIndex = 0; - PCNormals = Mat(PC.rows, 6, CV_32F); - +#if defined _OPENMP +#pragma omp parallel for +#endif for (i=0; i(i); - double nr[3]; - - int* indLocal = &indices[i*NumNeighbors]; + Matx33d C; + Vec3d mu; + const int* indLocal = Indices.ptr(i); // compute covariance matrix - meanCovLocalPCInd(dataset, indLocal, 3, NumNeighbors, C, mu); + meanCovLocalPCInd(PCNormals, indLocal, NumNeighbors, C, mu); // eigenvectors of covariance matrix - Mat cov(3, 3, CV_64F), eigVect, eigVal; - double* covData = (double*)cov.data; - covData[0] = C[0][0]; - covData[1] = C[0][1]; - covData[2] = C[0][2]; - covData[3] = C[1][0]; - covData[4] = C[1][1]; - covData[5] = C[1][2]; - covData[6] = C[2][0]; - covData[7] = C[2][1]; - covData[8] = C[2][2]; - eigen(cov, eigVal, eigVect); - Mat lowestEigVec; - //the eigenvector for the lowest eigenvalue is in the last row - eigVect.row(eigVect.rows - 1).copyTo(lowestEigVec); - double* eigData = (double*)lowestEigVec.data; - nr[0] = eigData[0]; - nr[1] = eigData[1]; - nr[2] = eigData[2]; - - pcr[0] = pci[0]; - pcr[1] = pci[1]; - pcr[2] = pci[2]; + Mat eigVect, eigVal; + eigen(C, eigVal, eigVect); + eigVect.row(2).convertTo(PCNormals.row(i).colRange(3, 6), CV_32F); if (FlipViewpoint) { - flipNormalViewpoint(pci, viewpoint[0], viewpoint[1], viewpoint[2], &nr[0], &nr[1], &nr[2]); + Vec3f nr(PCNormals.ptr(i) + 3); + Vec3f pci(PCNormals.ptr(i)); + flipNormalViewpoint(pci, viewpoint, nr); + Mat(nr).reshape(1, 1).copyTo(PCNormals.row(i).colRange(3, 6)); } - - pcr[3] = (float)nr[0]; - pcr[4] = (float)nr[1]; - pcr[5] = (float)nr[2]; } - delete[] indices; - delete[] distances; - delete[] dataset; - return 1; } diff --git a/modules/surface_matching/src/ppf_match_3d.cpp b/modules/surface_matching/src/ppf_match_3d.cpp index 258ce6a77..8f4f0c9cf 100644 --- a/modules/surface_matching/src/ppf_match_3d.cpp +++ b/modules/surface_matching/src/ppf_match_3d.cpp @@ -62,50 +62,47 @@ static int sortPoseClusters(const PoseCluster3DPtr& a, const PoseCluster3DPtr& b } // simple hashing -/*static int hashPPFSimple(const double f[4], const double AngleStep, const double DistanceStep) +/*static int hashPPFSimple(const Vec4d& f, const double AngleStep, const double DistanceStep) { - const unsigned char d1 = (unsigned char) (floor ((double)f[0] / (double)AngleStep)); - const unsigned char d2 = (unsigned char) (floor ((double)f[1] / (double)AngleStep)); - const unsigned char d3 = (unsigned char) (floor ((double)f[2] / (double)AngleStep)); - const unsigned char d4 = (unsigned char) (floor ((double)f[3] / (double)DistanceStep)); + Vec4i key( + (int)(f[0] / AngleStep), + (int)(f[1] / AngleStep), + (int)(f[2] / AngleStep), + (int)(f[3] / DistanceStep)); - int hashKey = (d1 | (d2<<8) | (d3<<16) | (d4<<24)); + int hashKey = d.val[0] | (d.val[1] << 8) | (d.val[2] << 16) | (d.val[3] << 24); return hashKey; }*/ // quantize ppf and hash it for proper indexing -static KeyType hashPPF(const double f[4], const double AngleStep, const double DistanceStep) +static KeyType hashPPF(const Vec4d& f, const double AngleStep, const double DistanceStep) { - const int d1 = (int) (floor ((double)f[0] / (double)AngleStep)); - const int d2 = (int) (floor ((double)f[1] / (double)AngleStep)); - const int d3 = (int) (floor ((double)f[2] / (double)AngleStep)); - const int d4 = (int) (floor ((double)f[3] / (double)DistanceStep)); - int key[4]={d1,d2,d3,d4}; - KeyType hashKey=0; - - murmurHash(key, 4*sizeof(int), 42, &hashKey); - + Vec4i key( + (int)(f[0] / AngleStep), + (int)(f[1] / AngleStep), + (int)(f[2] / AngleStep), + (int)(f[3] / DistanceStep)); + KeyType hashKey = 0; + + murmurHash(key.val, 4*sizeof(int), 42, &hashKey); return hashKey; } -/*static size_t hashMurmur(unsigned int key) +/*static size_t hashMurmur(uint key) { size_t hashKey=0; hashMurmurx86((void*)&key, 4, 42, &hashKey); return hashKey; }*/ -static double computeAlpha(const double p1[4], const double n1[4], const double p2[4]) +static double computeAlpha(const Vec3d& p1, const Vec3d& n1, const Vec3d& p2) { - double Tmg[3], mpt[3], row2[3], row3[3], alpha; - - computeTransformRTyz(p1, n1, row2, row3, Tmg); - - // checked row2, row3: They are correct - - mpt[1] = Tmg[1] + row2[0] * p2[0] + row2[1] * p2[1] + row2[2] * p2[2]; - mpt[2] = Tmg[2] + row3[0] * p2[0] + row3[1] * p2[1] + row3[2] * p2[2]; + Vec3d Tmg, mpt; + Matx33d R; + double alpha; + computeTransformRT(p1, n1, R, Tmg); + mpt = Tmg + R * p2; alpha=atan2(-mpt[2], mpt[1]); if ( alpha != alpha) @@ -167,35 +164,15 @@ void PPF3DDetector::setSearchParams(const double positionThreshold, const double } // compute per point PPF as in paper -void PPF3DDetector::computePPFFeatures(const double p1[4], const double n1[4], - const double p2[4], const double n2[4], - double f[4]) +void PPF3DDetector::computePPFFeatures(const Vec3d& p1, const Vec3d& n1, + const Vec3d& p2, const Vec3d& n2, + Vec4d& f) { - /* - Vectors will be defined as of length 4 instead of 3, because of: - - Further SIMD vectorization - - Cache alignment - */ - - double d[4] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2], 0}; - - double norm = TNorm3(d); - f[3] = norm; - - if (norm) - { - d[0] /= f[3]; - d[1] /= f[3]; - d[2] /= f[3]; - } - else - { - // TODO: Handle this - f[0] = 0; - f[1] = 0; - f[2] = 0; - return ; - } + Vec3d d(p2 - p1); + f[3] = cv::norm(d); + if (f[3] <= EPS) + return; + d *= 1.0 / f[3]; f[0] = TAngle3Normalized(n1, d); f[1] = TAngle3Normalized(n2, d); @@ -228,7 +205,7 @@ void PPF3DDetector::trainModel(const Mat &PC) CV_Assert(PC.type() == CV_32F || PC.type() == CV_32FC1); // compute bbox - float xRange[2], yRange[2], zRange[2]; + Vec2f xRange, yRange, zRange; computeBboxStd(PC, xRange, yRange, zRange); // compute sampling step from diameter of bbox @@ -261,9 +238,8 @@ void PPF3DDetector::trainModel(const Mat &PC) // since this is just a training part. for (int i=0; i(i); - const double p1[4] = {f1[0], f1[1], f1[2], 0}; - const double n1[4] = {f1[3], f1[4], f1[5], 0}; + const Vec3f p1(sampled.ptr(i)); + const Vec3f n1(sampled.ptr(i) + 3); //printf("///////////////////// NEW REFERENCE ////////////////////////\n"); for (int j=0; j(j); - const double p2[4] = {f2[0], f2[1], f2[2], 0}; - const double n2[4] = {f2[3], f2[4], f2[5], 0}; + const Vec3f p2(sampled.ptr(j)); + const Vec3f n2(sampled.ptr(j) + 3); - double f[4]={0}; + Vec4d f = Vec4d::all(0); computePPFFeatures(p1, n1, p2, n2, f); KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep); double alpha = computeAlpha(p1, n1, p2); - unsigned int ppfInd = i*numRefPoints+j; + uint ppfInd = i*numRefPoints+j; THash* hashNode = &hash_nodes[i*numRefPoints+j]; hashNode->id = hashValue; @@ -288,12 +263,8 @@ void PPF3DDetector::trainModel(const Mat &PC) hashtableInsertHashed(hashTable, hashValue, (void*)hashNode); - float* ppfRow = ppf.ptr(ppfInd); - ppfRow[0] = (float)f[0]; - ppfRow[1] = (float)f[1]; - ppfRow[2] = (float)f[2]; - ppfRow[3] = (float)f[3]; - ppfRow[4] = (float)alpha; + Mat(f).reshape(1, 1).convertTo(ppf.row(ppfInd).colRange(0, 4), CV_32F); + ppf.ptr(ppfInd)[4] = (float)alpha; } } } @@ -314,8 +285,8 @@ void PPF3DDetector::trainModel(const Mat &PC) bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose) { // translational difference - double dv[3] = {targetPose.t[0]-sourcePose.t[0], targetPose.t[1]-sourcePose.t[1], targetPose.t[2]-sourcePose.t[2]}; - double dNorm = sqrt(dv[0]*dv[0]+dv[1]*dv[1]+dv[2]*dv[2]); + Vec3d dv = targetPose.t - sourcePose.t; + double dNorm = cv::norm(dv); const double phi = fabs ( sourcePose.angle - targetPose.angle ); @@ -369,13 +340,14 @@ void PPF3DDetector::clusterPoses(std::vector& poseList, int numPoses, for (int i=0; i(poseClusters.size()); i++) { // We could only average the quaternions. So I will make use of them here - double qAvg[4]={0}, tAvg[3]={0}; + Vec4d qAvg = Vec4d::all(0); + Vec3d tAvg = Vec3d::all(0); // Perform the final averaging PoseCluster3DPtr curCluster = poseClusters[i]; std::vector curPoses = curCluster->poseList; int curSize = (int)curPoses.size(); - int numTotalVotes = 0; + size_t numTotalVotes = 0; for (int j=0; jnumVotes; @@ -386,25 +358,13 @@ void PPF3DDetector::clusterPoses(std::vector& poseList, int numPoses, { const double w = (double)curPoses[j]->numVotes / (double)numTotalVotes; - qAvg[0]+= w*curPoses[j]->q[0]; - qAvg[1]+= w*curPoses[j]->q[1]; - qAvg[2]+= w*curPoses[j]->q[2]; - qAvg[3]+= w*curPoses[j]->q[3]; - - tAvg[0]+= w*curPoses[j]->t[0]; - tAvg[1]+= w*curPoses[j]->t[1]; - tAvg[2]+= w*curPoses[j]->t[2]; - wSum+=w; + qAvg += w * curPoses[j]->q; + tAvg += w * curPoses[j]->t; + wSum += w; } - tAvg[0]/=wSum; - tAvg[1]/=wSum; - tAvg[2]/=wSum; - - qAvg[0]/=wSum; - qAvg[1]/=wSum; - qAvg[2]/=wSum; - qAvg[3]/=wSum; + tAvg *= 1.0 / wSum; + qAvg *= 1.0 / wSum; curPoses[0]->updatePoseQuat(qAvg, tAvg); curPoses[0]->numVotes=curCluster->numVotes; @@ -420,7 +380,8 @@ void PPF3DDetector::clusterPoses(std::vector& poseList, int numPoses, for (int i=0; i(poseClusters.size()); i++) { // We could only average the quaternions. So I will make use of them here - double qAvg[4]={0}, tAvg[3]={0}; + Vec4d qAvg = Vec4d::all(0); + Vec3d tAvg = Vec3d::all(0); // Perform the final averaging PoseCluster3DPtr curCluster = poseClusters[i]; @@ -429,24 +390,12 @@ void PPF3DDetector::clusterPoses(std::vector& poseList, int numPoses, for (int j=0; jq[0]; - qAvg[1]+= curPoses[j]->q[1]; - qAvg[2]+= curPoses[j]->q[2]; - qAvg[3]+= curPoses[j]->q[3]; - - tAvg[0]+= curPoses[j]->t[0]; - tAvg[1]+= curPoses[j]->t[1]; - tAvg[2]+= curPoses[j]->t[2]; + qAvg += curPoses[j]->q; + tAvg += curPoses[j]->t; } - tAvg[0]/=(double)curSize; - tAvg[1]/=(double)curSize; - tAvg[2]/=(double)curSize; - - qAvg[0]/=(double)curSize; - qAvg[1]/=(double)curSize; - qAvg[2]/=(double)curSize; - qAvg[3]/=(double)curSize; + tAvg *= 1.0 / curSize; + qAvg *= 1.0 / curSize; curPoses[0]->updatePoseQuat(qAvg, tAvg); curPoses[0]->numVotes=curCluster->numVotes; @@ -473,12 +422,12 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const //int numNeighbors = 10; int numAngles = (int) (floor (2 * M_PI / angle_step)); float distanceStep = (float)distance_step; - unsigned int n = num_ref_points; + uint n = num_ref_points; std::vector poseList; int sceneSamplingStep = scene_sample_step; // compute bbox - float xRange[2], yRange[2], zRange[2]; + Vec2f xRange, yRange, zRange; computeBboxStd(pc, xRange, yRange, zRange); // sample the point cloud @@ -491,7 +440,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const // allocate the accumulator : Moved this to the inside of the loop /*#if !defined (_OPENMP) - unsigned int* accumulator = (unsigned int*)calloc(numAngles*n, sizeof(unsigned int)); + uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint)); #endif*/ poseList.reserve((sampled.rows/sceneSamplingStep)+4); @@ -501,18 +450,16 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const #endif for (int i = 0; i < sampled.rows; i += sceneSamplingStep) { - unsigned int refIndMax = 0, alphaIndMax = 0; - unsigned int maxVotes = 0; + uint refIndMax = 0, alphaIndMax = 0; + uint maxVotes = 0; - float* f1 = sampled.ptr(i); - const double p1[4] = {f1[0], f1[1], f1[2], 0}; - const double n1[4] = {f1[3], f1[4], f1[5], 0}; - double *row2, *row3, tsg[3]={0}, Rsg[9]={0}, RInv[9]={0}; + const Vec3f p1(sampled.ptr(i)); + const Vec3f n1(sampled.ptr(i) + 3); + Vec3d tsg = Vec3d::all(0); + Matx33d Rsg = Matx33d::all(0), RInv = Matx33d::all(0); - unsigned int* accumulator = (unsigned int*)calloc(numAngles*n, sizeof(unsigned int)); + uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint)); computeTransformRT(p1, n1, Rsg, tsg); - row2=&Rsg[3]; - row3=&Rsg[6]; // Tolga Birdal's notice: // As a later update, we might want to look into a local neighborhood only @@ -523,19 +470,16 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const { if (i!=j) { - float* f2 = sampled.ptr(j); - const double p2[4] = {f2[0], f2[1], f2[2], 0}; - const double n2[4] = {f2[3], f2[4], f2[5], 0}; - double p2t[4], alpha_scene; + const Vec3f p2(sampled.ptr(j)); + const Vec3f n2(sampled.ptr(j) + 3); + Vec3d p2t; + double alpha_scene; - double f[4]={0}; + Vec4d f = Vec4d::all(0); computePPFFeatures(p1, n1, p2, n2, f); KeyType hashValue = hashPPF(f, angle_step, distanceStep); - // we don't need to call this here, as we already estimate the tsg from scene reference point - // double alpha = computeAlpha(p1, n1, p2); - p2t[1] = tsg[1] + row2[0] * p2[0] + row2[1] * p2[1] + row2[2] * p2[2]; - p2t[2] = tsg[2] + row3[0] * p2[0] + row3[1] * p2[1] + row3[2] * p2[2]; + p2t = tsg + Rsg * Vec3d(p2); alpha_scene=atan2(-p2t[2], p2t[1]); @@ -570,7 +514,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const //printf("%f\n", alpha); int alpha_index = (int)(numAngles*(alpha + 2*M_PI) / (4*M_PI)); - unsigned int accIndex = corrI * numAngles + alpha_index; + uint accIndex = corrI * numAngles + alpha_index; accumulator[accIndex]++; node = node->next; @@ -579,12 +523,12 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const } // Maximize the accumulator - for (unsigned int k = 0; k < n; k++) + for (uint k = 0; k < n; k++) { for (int j = 0; j < numAngles; j++) { - const unsigned int accInd = k*numAngles + j; - const unsigned int accVal = accumulator[ accInd ]; + const uint accInd = k*numAngles + j; + const uint accVal = accumulator[ accInd ]; if (accVal > maxVotes) { maxVotes = accVal; @@ -600,43 +544,35 @@ void PPF3DDetector::match(const Mat& pc, std::vector& results, const // invert Tsg : Luckily rotation is orthogonal: Inverse = Transpose. // We are not required to invert. - double tInv[3], tmg[3], Rmg[9]; - matrixTranspose33(Rsg, RInv); - matrixProduct331(RInv, tsg, tInv); + Vec3d tInv, tmg; + Matx33d Rmg; + RInv = Rsg.t(); + tInv = -RInv * tsg; - double TsgInv[16] = { RInv[0], RInv[1], RInv[2], -tInv[0], - RInv[3], RInv[4], RInv[5], -tInv[1], - RInv[6], RInv[7], RInv[8], -tInv[2], - 0, 0, 0, 1 - }; + Matx44d TsgInv; + rtToPose(RInv, tInv, TsgInv); // TODO : Compute pose - const float* fMax = sampled_pc.ptr(refIndMax); - const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1}; - const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1}; + const Vec3f pMax(sampled_pc.ptr(refIndMax)); + const Vec3f nMax(sampled_pc.ptr(refIndMax) + 3); computeTransformRT(pMax, nMax, Rmg, tmg); - row2=&Rsg[3]; - row3=&Rsg[6]; - double Tmg[16] = { Rmg[0], Rmg[1], Rmg[2], tmg[0], - Rmg[3], Rmg[4], Rmg[5], tmg[1], - Rmg[6], Rmg[7], Rmg[8], tmg[2], - 0, 0, 0, 1 - }; + Matx44d Tmg; + rtToPose(Rmg, tmg, Tmg); // convert alpha_index to alpha int alpha_index = alphaIndMax; double alpha = (alpha_index*(4*M_PI))/numAngles-2*M_PI; // Equation 2: - double Talpha[16]={0}; - getUnitXRotation_44(alpha, Talpha); + Matx44d Talpha; + Matx33d R; + Vec3d t = Vec3d::all(0); + getUnitXRotation(alpha, R); + rtToPose(R, t, Talpha); - double Temp[16]={0}; - double rawPose[16]={0}; - matrixProduct44(Talpha, Tmg, Temp); - matrixProduct44(TsgInv, Temp, rawPose); + Matx44d rawPose = TsgInv * (Talpha * Tmg); Pose3DPtr pose(new Pose3D(alpha, refIndMax, maxVotes)); pose->updatePose(rawPose); diff --git a/modules/surface_matching/src/t_hash_int.cpp b/modules/surface_matching/src/t_hash_int.cpp index 8e86e8159..587c65336 100644 --- a/modules/surface_matching/src/t_hash_int.cpp +++ b/modules/surface_matching/src/t_hash_int.cpp @@ -47,10 +47,10 @@ namespace ppf_match_3d // This magic value is just #define T_HASH_MAGIC 427462442 -size_t hash( unsigned int a); +size_t hash( uint a); // default hash function -size_t hash( unsigned int a) +size_t hash( uint a) { a = (a+0x7ed55d16) + (a<<12); a = (a^0xc761c23c) ^ (a>>19); @@ -61,37 +61,37 @@ size_t hash( unsigned int a) return a; } -hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int)) +hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(uint)) { hashtable_int *hashtbl; - + if (size < 16) { size = 16; } else { - size = (size_t)next_power_of_two((unsigned int)size); + size = (size_t)next_power_of_two((uint)size); } - - hashtbl=(hashtable_int*)malloc(sizeof(hashtable_int)); + + hashtbl=(hashtable_int*)malloc(sizeof(hashtable_int)); if (!hashtbl) return NULL; - - hashtbl->nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*)); + + hashtbl->nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*)); if (!hashtbl->nodes) { free(hashtbl); return NULL; } - + hashtbl->size=size; - + if (hashfunc) hashtbl->hashfunc=hashfunc; else hashtbl->hashfunc=hash; - + return hashtbl; } @@ -100,7 +100,7 @@ void hashtableDestroy(hashtable_int *hashtbl) { size_t n; struct hashnode_i *node, *oldnode; - + for (n=0; nsize; ++n) { node=hashtbl->nodes[n]; @@ -120,10 +120,10 @@ int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data) { struct hashnode_i *node; size_t hash=hashtbl->hashfunc(key)%hashtbl->size; - - + + /* fpruintf(stderr, "hashtbl_insert() key=%s, hash=%d, data=%s\n", key, hash, (char*)data);*/ - + node=hashtbl->nodes[hash]; while (node) { @@ -134,18 +134,18 @@ int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data) } node=node->next; } - - - node=(hashnode_i*)malloc(sizeof(struct hashnode_i)); + + + node=(hashnode_i*)malloc(sizeof(struct hashnode_i)); if (!node) return -1; node->key=key; - + node->data=data; node->next=hashtbl->nodes[hash]; hashtbl->nodes[hash]=node; - - + + return 0; } @@ -153,10 +153,10 @@ int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data) { struct hashnode_i *node; size_t hash = key % hashtbl->size; - - + + /* fpruintf(stderr, "hashtbl_insert() key=%s, hash=%d, data=%s\n", key, hash, (char*)data);*/ - + node=hashtbl->nodes[hash]; while (node) { @@ -167,18 +167,18 @@ int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data) } node=node->next; } - - node=(hashnode_i*)malloc(sizeof(struct hashnode_i)); + + node=(hashnode_i*)malloc(sizeof(struct hashnode_i)); if (!node) return -1; - + node->key=key; - + node->data=data; node->next=hashtbl->nodes[hash]; hashtbl->nodes[hash]=node; - - + + return 0; } @@ -187,7 +187,7 @@ int hashtableRemove(hashtable_int *hashtbl, KeyType key) { struct hashnode_i *node, *prevnode=NULL; size_t hash=hashtbl->hashfunc(key)%hashtbl->size; - + node=hashtbl->nodes[hash]; while (node) { @@ -203,7 +203,7 @@ int hashtableRemove(hashtable_int *hashtbl, KeyType key) prevnode=node; node=node->next; } - + return -1; } @@ -212,9 +212,9 @@ void *hashtableGet(hashtable_int *hashtbl, KeyType key) { struct hashnode_i *node; size_t hash=hashtbl->hashfunc(key)%hashtbl->size; - + /* fprintf(stderr, "hashtbl_get() key=%s, hash=%d\n", key, hash);*/ - + node=hashtbl->nodes[hash]; while (node) { @@ -222,14 +222,14 @@ void *hashtableGet(hashtable_int *hashtbl, KeyType key) return node->data; node=node->next; } - + return NULL; } hashnode_i* hashtableGetBucketHashed(hashtable_int *hashtbl, KeyType key) { size_t hash = key % hashtbl->size; - + return hashtbl->nodes[hash]; } @@ -238,14 +238,14 @@ int hashtableResize(hashtable_int *hashtbl, size_t size) hashtable_int newtbl; size_t n; struct hashnode_i *node,*next; - + newtbl.size=size; newtbl.hashfunc=hashtbl->hashfunc; - - newtbl.nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*)); + + newtbl.nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*)); if (!newtbl.nodes) return -1; - + for (n=0; nsize; ++n) { for (node=hashtbl->nodes[n]; node; node=next) @@ -253,14 +253,14 @@ int hashtableResize(hashtable_int *hashtbl, size_t size) next = node->next; hashtableInsert(&newtbl, node->key, node->data); hashtableRemove(hashtbl, node->key); - + } } - + free(hashtbl->nodes); hashtbl->size=newtbl.size; hashtbl->nodes=newtbl.nodes; - + return 0; } @@ -270,24 +270,24 @@ int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f size_t hashMagic=T_HASH_MAGIC; size_t n=hashtbl->size; size_t i; - + fwrite(&hashMagic, sizeof(size_t),1, f); fwrite(&n, sizeof(size_t),1, f); fwrite(&dataSize, sizeof(size_t),1, f); - + for (i=0; isize; i++) { struct hashnode_i* node=hashtbl->nodes[i]; size_t noEl=0; - + while (node) { noEl++; node=node->next; } - + fwrite(&noEl, sizeof(size_t),1, f); - + node=hashtbl->nodes[i]; while (node) { @@ -296,7 +296,7 @@ int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f node=node->next; } } - + return 1; } @@ -305,13 +305,13 @@ void hashtablePrint(hashtable_int *hashtbl) { size_t n; struct hashnode_i *node,*next; - + for (n=0; nsize; ++n) { for (node=hashtbl->nodes[n]; node; node=next) { next = node->next; - std::cout<<"Key : "<key<<", Data : "<data<key<<", Data : "<data<size; i++) { size_t j=0; status = fread(&n, sizeof(size_t),1, f); - + for (j=0; jsizeof(void*)) { data=malloc(dataSize); @@ -355,7 +355,7 @@ hashtable_int *hashtableRead(FILE* f) } else status = fread(&data, dataSize, 1, f); - + hashtableInsert(hashtbl, key, data); //free(key); } @@ -363,7 +363,7 @@ hashtable_int *hashtableRead(FILE* f) } else return 0; - + return hashtbl; }