Skip to content

O
opencv_contrib
Commit 995b6963 authored by Bence Magyar's avatar Bence Magyar
Browse files
Options

Cosmetic changes

parent d1193e31
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 323 additions and 330 deletions
modules/surface_matching/include/opencv2/surface_matching/icp.hpp
View file @ 995b6963
......@@ -42,8 +42,8 @@
/**
* @file icp.hpp
*
* @brief Implementation of ICP (Iterative Closest Point) Algorithm
* @author Tolga Birdal
* @brief Implementation of ICP (Iterative Closest Point) Algorithm
* @author Tolga Birdal
*/
#ifndef __OPENCV_SURFACE_MATCHING_ICP_HPP__
......@@ -66,93 +66,93 @@ namespace ppf_match_3d
* You will find that my emphasis is on the performance, while retaining the accuracy.
* This implementation is based on Tolga Birdal's MATLAB implementation in here:
* http://www.mathworks.com/matlabcentral/fileexchange/47152-icp-registration-using-efficient-variants-and-multi-resolution-scheme
The main contributions come from:
1. Picky ICP:
http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2003/Zinsser03-ARI.pdf
2. Efficient variants of the ICP Algorithm:
http://docs.happycoders.org/orgadoc/graphics/imaging/fasticp_paper.pdf
3. Geometrically Stable Sampling for the ICP Algorithm: https://graphics.stanford.edu/papers/stabicp/stabicp.pdf
4. Multi-resolution registration:
http://www.cvl.iis.u-tokyo.ac.jp/~oishi/Papers/Alignment/Jost_MultiResolutionICP_3DIM03.pdf
5. Linearization of Point-to-Plane metric by Kok Lim Low:
https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
*/
* The main contributions come from:
* 1. Picky ICP:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2003/Zinsser03-ARI.pdf
* 2. Efficient variants of the ICP Algorithm:
* http://docs.happycoders.org/orgadoc/graphics/imaging/fasticp_paper.pdf
* 3. Geometrically Stable Sampling for the ICP Algorithm: https://graphics.stanford.edu/papers/stabicp/stabicp.pdf
* 4. Multi-resolution registration:
* http://www.cvl.iis.u-tokyo.ac.jp/~oishi/Papers/Alignment/Jost_MultiResolutionICP_3DIM03.pdf
* 5. Linearization of Point-to-Plane metric by Kok Lim Low:
* https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
*/
class CV_EXPORTS ICP
{
public:
enum ICP_SAMPLING_TYPE
{
ICP_SAMPLING_TYPE_UNIFORM,
ICP_SAMPLING_TYPE_GELFAND
};
ICP()
{
m_tolerence = 0.005f;
m_rejectionScale = 2.5f;
m_maxItereations = 250;
m_numLevels = 6;
m_sampleType = ICP_SAMPLING_TYPE_UNIFORM;
m_numNeighborsCorr = 1;
}
virtual ~ICP() { }
/**
* \brief ICP constructor with default arguments.
* @param [in] tolerence Controls the accuracy of registration at each iteration of ICP.
* @param [in] rejectionScale Robust outlier rejection is applied for robustness. This value actually corresponds to the standard deviation coefficient. Points with rejectionScale * \sigma are ignored during registration.
* @param [in] numLevels Number of pyramid levels to proceed. Deep pyramids increase speed but decrease accuracy. Too coarse pyramids might have computational overhead on top of the inaccurate registrtaion. This parameter should be chosen to optimize a balance. Typical values range from 4 to 10.
* @param [in] sampleType Currently this parameter is ignored and only uniform sampling is applied. Leave it as 0.
* @param [in] numMaxCorr Currently this parameter is ignored and only PickyICP is applied. Leave it as 1.
* \return
*
* \details Constructor
*/
ICP(const int iterations, const float tolerence=0.05, const float rejectionScale=2.5, const int numLevels=6, const ICP_SAMPLING_TYPE sampleType = ICP_SAMPLING_TYPE_UNIFORM, const int numMaxCorr=1)
{
m_tolerence = tolerence;
m_numNeighborsCorr = numMaxCorr;
m_rejectionScale = rejectionScale;
m_maxItereations = iterations;
m_numLevels = numLevels;
m_sampleType = sampleType;
};
/**
* \brief Perform registration
*
* @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
* CV_32F is the only supported data type.
* @param [in] dstPC The input point cloud for the scene. It is assumed that the model is registered on the scene. Scene remains static. Expected to have the normals (Nx6). Currently, CV_32F is the only supported data type.
* @param [out] residual The output registration error.
* \return On successful termination, the function returns 0.
*
* \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
*/
int registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, double pose[16]);
/**
* \brief Perform registration with multiple initial poses
*
* @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
* CV_32F is the only supported data type.
* @param [in] dstPC The input point cloud for the scene. Currently, CV_32F is the only supported data type.
* @param [out] poses List output of poses. For more detailed information check out Pose3D.
* \return On successful termination, the function returns 0.
*
* \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
*/
int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses);
private:
float m_tolerence;
int m_maxItereations;
float m_rejectionScale;
int m_numNeighborsCorr;
int m_numLevels;
int m_sampleType;
public:
enum ICP_SAMPLING_TYPE
{
ICP_SAMPLING_TYPE_UNIFORM,
ICP_SAMPLING_TYPE_GELFAND
};
ICP()
{
m_tolerence = 0.005f;
m_rejectionScale = 2.5f;
m_maxItereations = 250;
m_numLevels = 6;
m_sampleType = ICP_SAMPLING_TYPE_UNIFORM;
m_numNeighborsCorr = 1;
}
virtual ~ICP() { }
/**
* \brief ICP constructor with default arguments.
* @param [in] tolerence Controls the accuracy of registration at each iteration of ICP.
* @param [in] rejectionScale Robust outlier rejection is applied for robustness. This value actually corresponds to the standard deviation coefficient. Points with rejectionScale * \sigma are ignored during registration.
* @param [in] numLevels Number of pyramid levels to proceed. Deep pyramids increase speed but decrease accuracy. Too coarse pyramids might have computational overhead on top of the inaccurate registrtaion. This parameter should be chosen to optimize a balance. Typical values range from 4 to 10.
* @param [in] sampleType Currently this parameter is ignored and only uniform sampling is applied. Leave it as 0.
* @param [in] numMaxCorr Currently this parameter is ignored and only PickyICP is applied. Leave it as 1.
* \return
*
* \details Constructor
*/
ICP(const int iterations, const float tolerence=0.05, const float rejectionScale=2.5, const int numLevels=6, const ICP_SAMPLING_TYPE sampleType = ICP_SAMPLING_TYPE_UNIFORM, const int numMaxCorr=1)
{
m_tolerence = tolerence;
m_numNeighborsCorr = numMaxCorr;
m_rejectionScale = rejectionScale;
m_maxItereations = iterations;
m_numLevels = numLevels;
m_sampleType = sampleType;
}
/**
* \brief Perform registration
*
* @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
* CV_32F is the only supported data type.
* @param [in] dstPC The input point cloud for the scene. It is assumed that the model is registered on the scene. Scene remains static. Expected to have the normals (Nx6). Currently, CV_32F is the only supported data type.
* @param [out] residual The output registration error.
* \return On successful termination, the function returns 0.
*
* \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
*/
int registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, double pose[16]);
/**
* \brief Perform registration with multiple initial poses
*
* @param [in] srcPC The input point cloud for the model. Expected to have the normals (Nx6). Currently,
* CV_32F is the only supported data type.
* @param [in] dstPC The input point cloud for the scene. Currently, CV_32F is the only supported data type.
* @param [out] poses List output of poses. For more detailed information check out Pose3D.
* \return On successful termination, the function returns 0.
*
* \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
*/
int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses);
private:
float m_tolerence;
int m_maxItereations;
float m_rejectionScale;
int m_numNeighborsCorr;
int m_numLevels;
int m_sampleType;
};
......
modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp
View file @ 995b6963
......@@ -57,68 +57,67 @@ namespace ppf_match_3d
*/
class CV_EXPORTS Pose3D
{
public:
Pose3D()
{
alpha=0;
modelIndex=0;
numVotes=0;
residual = 0;
for (int i=0; i<16; i++)
Pose[i]=0;
};
Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0)
{
alpha = Alpha;
modelIndex = ModelIndex;
numVotes = NumVotes;
residual=0;
for (int i=0; i<16; i++)
Pose[i]=0;
};
/**
* \brief Updates the pose with the new one
* \param [in] NewPose New pose to overwrite
*/
void updatePose(double NewPose[16]);
/**
* \brief Updates the pose with the new one
* \param [in] NewPose New pose to overwrite
*/
void updatePose(double NewR[9], double NewT[3]);
/**
* \brief Updates the pose with the new one, but this time using quaternions to represent rotation
* \param [in] NewPose New pose to overwrite
*/
void updatePoseQuat(double Q[4], double NewT[3]);
/**
* \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 printPose();
Pose3D* clone();
int writePose(FILE* f);
int readPose(FILE* f);
int writePose(const std::string& FileName);
int readPose(const std::string& FileName);
virtual ~Pose3D() {};
double alpha, residual;
unsigned int modelIndex;
unsigned int numVotes;
double Pose[16], angle, t[3], q[4];
public:
Pose3D()
{
alpha=0;
modelIndex=0;
numVotes=0;
residual = 0;
for (int i=0; i<16; i++)
Pose[i]=0;
}
Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0)
{
alpha = Alpha;
modelIndex = ModelIndex;
numVotes = NumVotes;
residual=0;
for (int i=0; i<16; i++)
Pose[i]=0;
}
/**
* \brief Updates the pose with the new one
* \param [in] NewPose New pose to overwrite
*/
void updatePose(double NewPose[16]);
/**
* \brief Updates the pose with the new one
* \param [in] NewPose New pose to overwrite
*/
void updatePose(double NewR[9], double NewT[3]);
/**
* \brief Updates the pose with the new one, but this time using quaternions to represent rotation
* \param [in] NewPose New pose to overwrite
*/
void updatePoseQuat(double Q[4], double NewT[3]);
/**
* \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 printPose();
Pose3D* clone();
int writePose(FILE* f);
int readPose(FILE* f);
int writePose(const std::string& FileName);
int readPose(const std::string& FileName);
virtual ~Pose3D() {}
double alpha, residual;
unsigned int modelIndex;
unsigned int numVotes;
double Pose[16], angle, t[3], q[4];
};
/**
......@@ -129,53 +128,48 @@ class CV_EXPORTS Pose3D
*/
class CV_EXPORTS PoseCluster3D
{
public:
PoseCluster3D()
{
//poseList.clear();
numVotes=0;
id=0;
};
PoseCluster3D(Pose3D* newPose)
{
//poseList.clear();
poseList.push_back(newPose);
numVotes=newPose->numVotes;
id=0;
};
PoseCluster3D(Pose3D* newPose, int newId)
{
//poseList.clear();
poseList.push_back(newPose);
this->numVotes = newPose->numVotes;
this->id = newId;
};
virtual ~PoseCluster3D()
{
numVotes=0;
id=0;
//poseList.clear();
};
/**
* \brief Adds a new pose to the cluster. The pose should be "close" to the mean poses
* in order to preserve the consistency
* \param [in] newPose Pose to add to the cluster
*/
void addPose(Pose3D* newPose) ;
int writePoseCluster(FILE* f);
int readPoseCluster(FILE* f);
int writePoseCluster(const std::string& FileName);
int readPoseCluster(const std::string& FileName);
std::vector < Pose3D* > poseList;
int numVotes;
int id;
public:
PoseCluster3D()
{
numVotes=0;
id=0;
}
PoseCluster3D(Pose3D* newPose)
{
poseList.clear();
poseList.push_back(newPose);
numVotes=newPose->numVotes;
id=0;
}
PoseCluster3D(Pose3D* newPose, int newId)
{
poseList.push_back(newPose);
this->numVotes = newPose->numVotes;
this->id = newId;
}
virtual ~PoseCluster3D()
{}
/**
* \brief Adds a new pose to the cluster. The pose should be "close" to the mean poses
* in order to preserve the consistency
* \param [in] newPose Pose to add to the cluster
*/
void addPose(Pose3D* newPose);
int writePoseCluster(FILE* f);
int readPoseCluster(FILE* f);
int writePoseCluster(const std::string& FileName);
int readPoseCluster(const std::string& FileName);
std::vector < Pose3D* > poseList;
int numVotes;
int id;
};
} // namespace ppf_match_3d
} // namespace cv
......
modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp
View file @ 995b6963
......@@ -53,33 +53,33 @@ typedef unsigned int KeyType;
typedef struct hashnode_i
{
KeyType key;
void *data;
struct hashnode_i *next;
KeyType key;
void *data;
struct hashnode_i *next;
} hashnode_i ;
typedef struct HSHTBL_i
{
size_t size;
struct hashnode_i **nodes;
size_t (*hashfunc)(unsigned int);
size_t size;
struct hashnode_i **nodes;
size_t (*hashfunc)(unsigned int);
} hashtable_int;
inline static unsigned int next_power_of_two(unsigned int value)
{
/* Round up to the next highest power of 2 */
/* from http://www-graphics.stanford.edu/~seander/bithacks.html */
--value;
value |= value >> 1;
value |= value >> 2;
value |= value >> 4;
value |= value >> 8;
value |= value >> 16;
++value;
return value;
/* Round up to the next highest power of 2 */
/* from http://www-graphics.stanford.edu/~seander/bithacks.html */
--value;
value |= value >> 1;
value |= value >> 2;
value |= value >> 4;
value |= value >> 8;
value |= value >> 16;
++value;
return value;
}
hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int));
......
modules/surface_matching/samples/ppf_load_match.cpp
View file @ 995b6963
......@@ -47,24 +47,24 @@ using namespace std;
using namespace cv;
using namespace ppf_match_3d;
static void help(std::string errorMessage)
static void help(const string& errorMessage)
{
std::cout<<"Program init error : "<<errorMessage<<std::endl;
std::cout<<"\nUsage : ppf_matching [input model file] [input scene file]"<<std::endl;
std::cout<<"\nPlease start again with new parameters"<<std::endl;
cout << "Program init error : "<< errorMessage << endl;
cout << "\nUsage : ppf_matching [input model file] [input scene file]"<< endl;
cout << "\nPlease start again with new parameters"<< endl;
}
int main(int argc, char** argv)
{
// welcome message
std::cout<< "****************************************************"<<std::endl;
std::cout<< "* Surface Matching demonstration : demonstrates the use of surface matching"
" using point pair features."<<std::endl;
std::cout<< "* The sample loads a model and a scene, where the model lies in a different"
cout << "****************************************************" << endl;
cout << "* Surface Matching demonstration : demonstrates the use of surface matching"
" using point pair features." << endl;
cout << "* The sample loads a model and a scene, where the model lies in a different"
" pose than the training.\n* It then trains the model and searches for it in the"
" input scene. The detected poses are further refined by ICP\n* and printed to the "
" standard output."<<std::endl;
std::cout<< "****************************************************"<<std::endl;
" standard output." << endl;
cout << "****************************************************" << endl;
if (argc < 3)
{
......@@ -73,15 +73,15 @@ int main(int argc, char** argv)
}
#if (defined __x86_64__ || defined _M_X64)
std::cout << "Running on 64 bits" << std::endl;
cout << "Running on 64 bits" << endl;
#else
std::cout << "Running on 32 bits" << std::endl;
cout << "Running on 32 bits" << endl;
#endif
#ifdef _OPENMP
std::cout << "Running with OpenMP" << std::endl;
cout << "Running with OpenMP" << endl;
#else
std::cout << "Running without OpenMP and without TBB" << std::endl;
cout << "Running without OpenMP and without TBB" << endl;
#endif
string modelFileName = (string)argv[1];
......
modules/surface_matching/src/c_utils.hpp
View file @ 995b6963
This diff is collapsed.
modules/surface_matching/src/hash_murmur64.hpp
View file @ 995b6963
......@@ -8,7 +8,7 @@
FORCE_INLINE unsigned int getblock ( const unsigned int * p, int i )
{
return p[i];
return p[i];
}
//----------
......@@ -18,125 +18,125 @@ FORCE_INLINE unsigned int getblock ( const unsigned int * p, int i )
FORCE_INLINE unsigned int fmix32 ( unsigned int h )
{
h ^= h >> 16;
h *= 0x85ebca6b;
h ^= h >> 13;
h *= 0xc2b2ae35;
h ^= h >> 16;
return h;
h ^= h >> 16;
h *= 0x85ebca6b;
h ^= h >> 13;
h *= 0xc2b2ae35;
h ^= h >> 16;
return h;
}
//-----------------------------------------------------------------------------
FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & k1, unsigned int & c1, unsigned int & c2 )
{
k1 *= c1;
k1 = ROTL32(k1,11);
k1 *= c2;
h1 ^= k1;
h1 = h1*3+0x52dce729;
c1 = c1*5+0x7b7d159c;
c2 = c2*5+0x6bce6396;
k1 *= c1;
k1 = ROTL32(k1,11);
k1 *= c2;
h1 ^= k1;
h1 = h1*3+0x52dce729;
c1 = c1*5+0x7b7d159c;
c2 = c2*5+0x6bce6396;
}
//-----------------------------------------------------------------------------
FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & h2, unsigned int & k1, unsigned int & k2, unsigned int & c1, unsigned int & c2 )
{
k1 *= c1;
k1 = ROTL32(k1,11);
k1 *= c2;
h1 ^= k1;
h1 += h2;
h2 = ROTL32(h2,17);
k2 *= c2;
k2 = ROTL32(k2,11);
k2 *= c1;
h2 ^= k2;
h2 += h1;
h1 = h1*3+0x52dce729;
h2 = h2*3+0x38495ab5;
c1 = c1*5+0x7b7d159c;
c2 = c2*5+0x6bce6396;
k1 *= c1;
k1 = ROTL32(k1,11);
k1 *= c2;
h1 ^= k1;
h1 += h2;
h2 = ROTL32(h2,17);
k2 *= c2;
k2 = ROTL32(k2,11);
k2 *= c1;
h2 ^= k2;
h2 += h1;
h1 = h1*3+0x52dce729;
h2 = h2*3+0x38495ab5;
c1 = c1*5+0x7b7d159c;
c2 = c2*5+0x6bce6396;
}
//----------
FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigned int seed, void * out )
{
const unsigned char * data = (const unsigned char*)key;
const int nblocks = len / 8;
unsigned int h1 = 0x8de1c3ac ^ seed;
unsigned int h2 = 0xbab98226 ^ seed;
unsigned int c1 = 0x95543787;
unsigned int c2 = 0x2ad7eb25;
//----------
// body
const unsigned int * blocks = (const unsigned int *)(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);
bmix32(h1,h2,k1,k2,c1,c2);
}
//----------
// tail
const unsigned char * tail = (const unsigned char*)(data + nblocks*8);
unsigned int k1 = 0;
unsigned int k2 = 0;
switch (len & 7)
{
case 7:
k2 ^= tail[6] << 16;
case 6:
k2 ^= tail[5] << 8;
case 5:
k2 ^= tail[4] << 0;
case 4:
k1 ^= tail[3] << 24;
case 3:
k1 ^= tail[2] << 16;
case 2:
k1 ^= tail[1] << 8;
case 1:
k1 ^= tail[0] << 0;
bmix32(h1,h2,k1,k2,c1,c2);
};
//----------
// finalization
h2 ^= len;
h1 += h2;
h2 += h1;
h1 = fmix32(h1);
h2 = fmix32(h2);
h1 += h2;
h2 += h1;
((unsigned int*)out)[0] = h1;
((unsigned int*)out)[1] = h2;
const unsigned char * data = (const unsigned char*)key;
const int nblocks = len / 8;
unsigned int h1 = 0x8de1c3ac ^ seed;
unsigned int h2 = 0xbab98226 ^ seed;
unsigned int c1 = 0x95543787;
unsigned int c2 = 0x2ad7eb25;
//----------
// body
const unsigned int * blocks = (const unsigned int *)(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);
bmix32(h1,h2,k1,k2,c1,c2);
}
//----------
// tail
const unsigned char * tail = (const unsigned char*)(data + nblocks*8);
unsigned int k1 = 0;
unsigned int k2 = 0;
switch (len & 7)
{
case 7:
k2 ^= tail[6] << 16;
case 6:
k2 ^= tail[5] << 8;
case 5:
k2 ^= tail[4] << 0;
case 4:
k1 ^= tail[3] << 24;
case 3:
k1 ^= tail[2] << 16;
case 2:
k1 ^= tail[1] << 8;
case 1:
k1 ^= tail[0] << 0;
bmix32(h1,h2,k1,k2,c1,c2);
};
//----------
// finalization
h2 ^= len;
h1 += h2;
h2 += h1;
h1 = fmix32(h1);
h2 = fmix32(h2);
h1 += h2;
h2 += h1;
((unsigned int*)out)[0] = h1;
((unsigned int*)out)[1] = h2;
}
#endif
\ No newline at end of file
#endif
modules/surface_matching/src/icp.cpp
View file @ 995b6963
This diff is collapsed.
modules/surface_matching/src/pose_3d.cpp
View file @ 995b6963
This diff is collapsed.
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