Skip to content

O
opencv_contrib
Commit 2cfc3531 authored by Hamdi Sahloul's avatar Hamdi Sahloul Committed by Vitaly Tuzov
Browse files
Options

Re-write surface matching using vectors and matrices

parent 41995b76
Hide whitespace changes
Inline Side-by-side
Showing with 506 additions and 1149 deletions
modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp
View file @ 2cfc3531
......@@ -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<Pose3DPtr> poseList;
int numVotes;
size_t numVotes;
int id;
};
......
modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp
View file @ 2cfc3531
......@@ -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<int>& indices);
......@@ -94,26 +94,15 @@ Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& 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);
//! @}
......
modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp
View file @ 2cfc3531
......@@ -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);
......
modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp
View file @ 2cfc3531
......@@ -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);
......
modules/surface_matching/src/c_utils.hpp
View file @ 2cfc3531
......@@ -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
......
modules/surface_matching/src/hash_murmur64.hpp
View file @ 2cfc3531
......@@ -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;
}
......
modules/surface_matching/src/hash_murmur86.hpp
View file @ 2cfc3531
......@@ -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 <limits.h>
#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);
}
modules/surface_matching/src/icp.cpp
View file @ 2cfc3531
......@@ -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<Src.rows; i++)
{
const double *srcPt = Src.ptr<double>(i);
const double *dstPt = Dst.ptr<double>(i);
const double *normals = &dstPt[3];
double *bVal = b.ptr<double>(i);
double *aRow = A.ptr<double>(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<double>(i));
const Vec3d dstPt(Dst.ptr<double>(i));
const Vec3d normals(Dst.ptr<double>(i) + 3);
const Vec3d sub = dstPt - srcPt;
const Vec3d axis = srcPt.cross(normals);
*b.ptr<double>(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<double>(0,0);
Pose[1] = DCM.at<double>(0,1);
Pose[2] = DCM.at<double>(0,2);
Pose[4] = DCM.at<double>(1,0);
Pose[5] = DCM.at<double>(1,1);
Pose[6] = DCM.at<double>(1,2);
Pose[8] = DCM.at<double>(2,0);
Pose[9] = DCM.at<double>(2,1);
Pose[10] = DCM.at<double>(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))) && i<MaxIterationsPyr)
{
......@@ -470,10 +419,11 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
}
}
Mat X;
minimizePointToPlaneMetric(Src_Match, Dst_Match, X);
getTransformMat(X, PoseX);
Vec3d rpy, t;
minimizePointToPlaneMetric(Src_Match, Dst_Match, rpy, t);
if (cvIsNaN(cv::trace(rpy)) || cvIsNaN(cv::norm(t)))
break;
getTransformMat(rpy, t, PoseX);
Src_Moved = transformPCPose(srcPCT, PoseX);
double fval = cv::norm(Src_Match, Dst_Match)/(double)(Src_Moved.rows);
......@@ -494,13 +444,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
}
double TempPose[16];
matrixProduct44(PoseX, pose.val, TempPose);
// no need to copy the last 4 rows
for (int c=0; c<12; c++)
pose.val[c] = TempPose[c];
pose = PoseX * pose;
residual = tempResidual;
delete[] newI;
......@@ -514,18 +458,11 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
destroyFlann(flann);
}
// Pose(1:3, 4) = Pose(1:3, 4)./scale;
pose.val[3] = pose.val[3]/scale + meanAvg[0];
pose.val[7] = pose.val[7]/scale + meanAvg[1];
pose.val[11] = pose.val[11]/scale + meanAvg[2];
// In MATLAB this would be : Pose(1:3, 4) = Pose(1:3, 4)./scale + meanAvg' - Pose(1:3, 1:3)*meanAvg';
double Rpose[9], Cpose[3];
poseToR(pose.val, Rpose);
matrixProduct331(Rpose, meanAvg, Cpose);
pose.val[3] -= Cpose[0];
pose.val[7] -= Cpose[1];
pose.val[11] -= Cpose[2];
Matx33d Rpose;
Vec3d Cpose;
poseToRT(pose, Rpose, Cpose);
Cpose = Cpose / scale + meanAvg - Rpose * meanAvg;
rtToPose(Rpose, Cpose, pose);
residual = tempResidual;
......@@ -543,7 +480,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po
Matx44d poseICP = Matx44d::eye();
Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
poses[i]->appendPose(poseICP.val);
poses[i]->appendPose(poseICP);
}
return 0;
}
......
modules/surface_matching/src/pose_3d.cpp
View file @ 2cfc3531
......@@ -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<Pose3D> 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;
}
......
modules/surface_matching/src/ppf_helpers.cpp
View file @ 2cfc3531
......@@ -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<std::string> split(const std::string &text, char sep) {
std::vector<std::string> tokens;
......@@ -183,7 +183,7 @@ void writePLY(Mat PC, const char* FileName)
{
const float* point = PC.ptr<float>(pi);
outFile << point[0] << " "<<point[1]<<" "<<point[2];
outFile << point[0] << " " << point[1] << " " << point[2];
if (vertNum==6)
{
......@@ -238,7 +238,7 @@ void writePLYVisibleNormals(Mat PC, const char* FileName)
if (hasNormals)
{
outFile << " 127 127 127" << std::endl;
outFile << point[0]+point[3] << " " << point[1]+point[4] << " " << point[2]+point[5];
outFile << point[0] + point[3] << " " << point[1] + point[4] << " " << point[2] + point[5];
outFile << " 255 0 0";
}
......@@ -306,7 +306,7 @@ void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const
// uses a volume instead of an octree
// TODO: Right now normals are required.
// This is much faster than sample_pc_octree
Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrange[2], float sampleStep, int weightByCenter)
Mat samplePCByQuantization(Mat pc, Vec2f& xrange, Vec2f& yrange, Vec2f& zrange, float sampleStep, int weightByCenter)
{
std::vector< std::vector<int> > 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<pc.rows; i++)
{
const float *pcData = pc.ptr<float>(i);
float *pcDataT = pct.ptr<float>(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<float>(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<PC.rows; i++)
{
const float* src = PC.ptr<float>(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<PC.rows; i++)
{
double C[3][3], mu[4];
const float* pci = &dataset[i*3];
float* pcr = PCNormals.ptr<float>(i);
double nr[3];
int* indLocal = &indices[i*NumNeighbors];
Matx33d C;
Vec3d mu;
const int* indLocal = Indices.ptr<int>(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<float>(i) + 3);
Vec3f pci(PCNormals.ptr<float>(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;
}
......
modules/surface_matching/src/ppf_match_3d.cpp
View file @ 2cfc3531
......@@ -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<numRefPoints; i++)
{
float* f1 = sampled.ptr<float>(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<float>(i));
const Vec3f n1(sampled.ptr<float>(i) + 3);
//printf("///////////////////// NEW REFERENCE ////////////////////////\n");
for (int j=0; j<numRefPoints; j++)
......@@ -271,15 +247,14 @@ void PPF3DDetector::trainModel(const Mat &PC)
// cannnot compute the ppf with myself
if (i!=j)
{
float* f2 = sampled.ptr<float>(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<float>(j));
const Vec3f n2(sampled.ptr<float>(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<float>(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<float>(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<Pose3DPtr>& poseList, int numPoses,
for (int i=0; i<static_cast<int>(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<Pose3DPtr> curPoses = curCluster->poseList;
int curSize = (int)curPoses.size();
int numTotalVotes = 0;
size_t numTotalVotes = 0;
for (int j=0; j<curSize; j++)
numTotalVotes += curPoses[j]->numVotes;
......@@ -386,25 +358,13 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& 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<Pose3DPtr>& poseList, int numPoses,
for (int i=0; i<static_cast<int>(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<Pose3DPtr>& poseList, int numPoses,
for (int j=0; j<curSize; j++)
{
qAvg[0]+= curPoses[j]->q[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<Pose3DPtr>& 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<Pose3DPtr> 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<Pose3DPtr>& 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<Pose3DPtr>& 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<float>(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<float>(i));
const Vec3f n1(sampled.ptr<float>(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<Pose3DPtr>& results, const
{
if (i!=j)
{
float* f2 = sampled.ptr<float>(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<float>(j));
const Vec3f n2(sampled.ptr<float>(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<Pose3DPtr>& 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<Pose3DPtr>& 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<Pose3DPtr>& 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<float>(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<float>(refIndMax));
const Vec3f nMax(sampled_pc.ptr<float>(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);
......
modules/surface_matching/src/t_hash_int.cpp
View file @ 2cfc3531
......@@ -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; n<hashtbl->size; ++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; n<hashtbl->size; ++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; i<hashtbl->size; 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; n<hashtbl->size; ++n)
{
for (node=hashtbl->nodes[n]; node; node=next)
{
next = node->next;
std::cout<<"Key : "<<node->key<<", Data : "<<node->data<<std::endl;
std::cout<<"Key : "<<node->key<<", Data : "<<node->data<<std::endl;
}
}
}
......@@ -321,7 +321,7 @@ hashtable_int *hashtableRead(FILE* f)
size_t hashMagic = 0;
size_t n = 0, status;
hashtable_int *hashtbl = 0;
status = fread(&hashMagic, sizeof(size_t),1, f);
if (status && hashMagic==T_HASH_MAGIC)
{
......@@ -329,20 +329,20 @@ hashtable_int *hashtableRead(FILE* f)
size_t dataSize;
status = fread(&n, sizeof(size_t),1, f);
status = fread(&dataSize, sizeof(size_t),1, f);
hashtbl=hashtableCreate(n, hash);
for (i=0; i<hashtbl->size; i++)
{
size_t j=0;
status = fread(&n, sizeof(size_t),1, f);
for (j=0; j<n; j++)
{
int key=0;
void* data=0;
status = fread(&key, sizeof(KeyType), 1, f);
if (dataSize>sizeof(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;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment