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
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 206 additions and 353 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
This diff is collapsed.
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/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