Skip to content

O
opencv_contrib
Commit 607f2bfc authored by Bence Magyar's avatar Bence Magyar
Browse files
Options

Add surface_matching module.

parent 12e1e11d
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 2456 additions and 0 deletions
modules/surface_matching/CMakeLists.txt 0 → 100644
View file @ 607f2bfc
set(the_description "3D point features")
ocv_define_module(surface_matching opencv_core opencv_flann)
modules/surface_matching/doc/surface_matching.rst 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/doc/surface_matching/src/ppf_load_match.cpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#include "opencv2/ppf_match_3d.hpp"
#include <iostream>
#include "opencv2/icp.hpp"
#include "opencv2/surface_matching/ppf_helpers.hpp"
#include "opencv2/core/utility.hpp"
using namespace std;
using namespace cv;
using namespace ppf_match_3d;
static void help(std::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;
}
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"
" pose than the training. It then "<<std::endl;
std::cout<< "****************************************************"<<std::endl;
if (argc < 3)
{
help("Not enough input arguments");
exit(1);
}
string modelFileName = (string)argv[1];
string sceneFileName = (string)argv[2];
Mat pc = loadPLYSimple(modelFileName.c_str(), 1);
// Now train the model
cout << "Training..." << endl;
int64 tick1 = cv::getTickCount();
ppf_match_3d::PPF3DDetector detector(0.03, 0.05);
detector.trainModel(pc);
int64 tick2 = cv::getTickCount();
cout << endl << "Training complete in "
<< (double)(tick2-tick1)/ cv::getTickFrequency()
<< " ms" << endl << "Loading model..." << endl;
// Read the scene
Mat pcTest = loadPLYSimple(sceneFileName.c_str(), 1);
// Match the model to the scene and get the pose
cout << endl << "Starting matching..." << endl;
vector < Pose3D* > results;
tick1 = cv::getTickCount();
detector.match(pcTest, results, 1.0/10.0, 0.05);
tick2 = cv::getTickCount();
cout << endl << "PPF Elapsed Time " <<
(tick2-tick1)/cv::getTickFrequency() << " ms" << endl;
// Get only first N results
int N = 2;
vector<Pose3D*>::const_iterator first = results.begin();
vector<Pose3D*>::const_iterator last = results.begin() + N;
vector<Pose3D*> resultsSub(first, last);
// Create an instance of ICP
ICP icp(200, 0.001f, 2.5f, 8);
int64 t1 = cv::getTickCount();
// Register for all selected poses
cout << endl << "Performing ICP on " << N << " poses..." << endl;
icp.registerModelToScene(pc, pcTest, resultsSub);
int64 t2 = cv::getTickCount();
cout << endl << "ICP Elapsed Time " <<
(t2-t1)/cv::getTickFrequency() << " sec" << endl;
cout << "Poses: " << endl;
// debug first five poses
for (size_t i=0; i<resultsSub.size(); i++)
{
Pose3D* pose = resultsSub[i];
cout << "Pose Result " << i << endl;
pose->printPose();
}
return 0;
}
modules/surface_matching/include/opencv2/icp.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
/**
* @file icp.hpp
*
* @brief Implementation of ICP (Iterative Closest Point) Algorithm
* @author Tolga Birdal
*/
#ifndef __OPENCV_ICP_HPP__
#define __OPENCV_ICP_HPP__
#include <opencv2/core.hpp>
#include "surface_matching/pose_3d.hpp"
#include <vector>
namespace cv
{
namespace ppf_match_3d
{
/**
* @class ICP
* @brief This class implements a very efficient and robust variant of the iterative closest point (ICP) algorithm.
* The task is to register a 3D model (or point cloud) against a set of noisy target data. The variants are put together
* by myself after certain tests. The task is to be able to match partial, noisy point clouds in cluttered scenes, quickly.
* 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
*/
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;
};
} // namespace ppf_match_3d
} // namespace cv
#endif
modules/surface_matching/include/opencv2/ppf_match_3d.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
/**
** ppf_match_3d : Interfaces for matching 3d surfaces in 3d scenes. This module implements the algorithm from Bertram Drost and Slobodan Ilic.
** Use: Read a 3D model, load a 3D scene and match the model to the scene
**
**
** Creation - 2014
** Author: Tolga Birdal (tbirdal@gmail.com)
**
** Refer to the following research paper for more information:
** B. Drost, Markus Ulrich, N. Navab, S. Ilic
Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR), San Francisco, California (USA), June 2010.
***/
// Author: Tolga Birdal <tbirdal AT gmail.com>
#ifndef __OPENCV_OBJDETECT_PPF_3D_HPP__
#define __OPENCV_OBJDETECT_PPF_3D_HPP__
#include <opencv2/core.hpp>
#include <vector>
#include "surface_matching/pose_3d.hpp"
#include "surface_matching/t_hash_int.hpp"
namespace cv
{
namespace ppf_match_3d
{
#define T_PPF_LENGTH 5
/**
* @struct THash
* @brief Struct, holding a node in the hashtable
*/
typedef struct THash
{
int id;
int i, ppfInd;
} THash;
/**
* @class PPF3DDetector
* @brief Class, allowing the load and matching 3D models.
* Typical Use:
*
* // Train a model
* ppf_match_3d::PPF3DDetector detector(0.05, 0.05);
* detector.trainModel(pc);
* // Search the model in a given scene
* vector < Pose3D* > results;
* detector.match(pcTest, results, 1.0/5.0,0.05);
*
*/
class CV_EXPORTS PPF3DDetector
{
public:
/**
* \brief Empty constructor. Sets default arguments
*/
PPF3DDetector();
/**
* Constructor with arguments
* @param [in] relativeSamplingStep Sampling distance relative to the object's diameter. Models are first sampled uniformly in order to improve efficiency. Decreasing this value leads to a denser model, and a more accurate pose estimation but the larger the model, the slower the training. Increasing the value leads to a less accurate pose computation but a smaller model and faster model generation and matching. Beware of the memory consumption when using small values.
* @param [in] relativeDistanceStep The discretization distance of the point pair distance relative to the model's diameter. This value has a direct impact on the hashtable. Using small values would lead to too fine discretization, and thus ambiguity in the bins of hashtable. Too large values would lead to no discrimination over the feature vectors and different point pair features would be assigned to the same bin. This argument defaults to the value of RelativeSamplingStep. For noisy scenes, the value can be increased to improve the robustness of the matching against noisy points.
* @param [in] numAngles Set the discretization of the point pair orientation as the number of subdivisions of the angle. This value is the equivalent of RelativeDistanceStep for the orientations. Increasing the value increases the precision of the matching but decreases the robustness against incorrect normal directions. Decreasing the value decreases the precision of the matching but increases the robustness against incorrect normal directions. For very noisy scenes where the normal directions can not be computed accurately, the value can be set to 25 or 20.
*/
PPF3DDetector(const double relativeSamplingStep, const double relativeDistanceStep=0.05, const double numAngles=30);
virtual ~PPF3DDetector();
/**
* Set the parameters for the search
* @param [in] numPoses The maximum number of poses to return
* @param [in] positionThreshold Position threshold controlling the similarity of translations. Depends on the units of calibration/model.
* @param [in] rotationThreshold Position threshold controlling the similarity of rotations. This parameter can be perceived as a threshold over the difference of angles
* @param [in] minMatchScore Not used at the moment
* @param [in] useWeightedClustering The algorithm by default clusters the poses without weighting. A non-zero value would indicate that the pose clustering should take into account the number of votes as the weights and perform a weighted averaging instead of a simple one.
*/
void setSearchParams(const int numPoses=5, const double positionThreshold=-1, const double rotationThreshold=-1, const double minMatchScore=0.5, const bool useWeightedClustering=false);
/**
* \brief Trains a new model.
*
* @param [in] Model The input point cloud with normals (Nx6)
*
* \details Uses the parameters set in the constructor to downsample and learn a new model. When the model is learnt, the instance gets ready for calling "match".
*/
void trainModel(const Mat& Model);
/**
* \brief Matches a trained model across a provided scene.
*
* @param [in] scene Point cloud for the scene
* @param [out] results List of output poses
* @param [in] relativeSceneSampleStep The ratio of scene points to be used for the matching after sampling with relativeSceneDistance. For example, if this value is set to 1.0/5.0, every 5th point from the scene is used for pose estimation. This parameter allows an easy trade-off between speed and accuracy of the matching. Increasing the value leads to less points being used and in turn to a faster but less accurate pose computation. Decreasing the value has the inverse effect.
* @param [in] relativeSceneDistance Set the distance threshold relative to the diameter of the model. This parameter is equivalent to relativeSamplingStep in the training stage. This parameter acts like a prior sampling with the relativeSceneSampleStep parameter.
*/
void match(const Mat& scene, std::vector<Pose3D*>& results, const double relativeSceneSampleStep=1.0/5.0, const double relativeSceneDistance=0.03);
void read(const FileNode& fn);
void write(FileStorage& fs) const;
protected:
double maxDist, angle_step, angleStepRadians, distance_step;
double samplingStepRelative, angleStepRelative, distanceStepRelative;
Mat inputPC, sampledPC, PPF;
int num_ref_points, sampled_step, ppf_step;
hashtable_int* hash_table;
THash* hash_nodes;
int NumPoses;
double PositionThreshold, RotationThreshold, MinMatchScore;
bool UseWeightedAvg;
float sampleStepSearch;
int SceneSampleStep;
void clearTrainingModels();
private:
void computePPFFeatures( const double p1[4], const double n1[4],
const double p2[4], const double n2[4],
double f[4]);
bool matchPose(const Pose3D& sourcePose, const Pose3D& targetPose);
int clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pose3D*>& finalPoses);
bool trained;
};
} // namespace ppf_match_3d
} // namespace cv
#endif
modules/surface_matching/include/opencv2/surface_matching.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
#ifndef __OPENCV_SURFACE_MATCHING_HPP__
#define __OPENCV_SURFACE_MATCHING_HPP__
#include "ppf_match_3d.hpp"
#include "icp.hpp"
#endif
modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#ifndef _OPENCV_POSE3D_HPP_
#define _OPENCV_POSE3D_HPP_
#include <vector>
#include <string>
namespace cv
{
namespace ppf_match_3d
{
/**
* @class Pose3D
* @brief Class, allowing the storage of a pose. The data structure stores both
* the quaternions and the matrix forms. It supports IO functionality together with
* various helper methods to work with poses
*
*/
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];
};
/**
* @class PoseCluster3D
* @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
{
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;
};
} // namespace ppf_match_3d
} // namespace cv
#endif
modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#ifndef __OPENCV_PPF_3D_HELPERS_HPP__
#define __OPENCV_PPF_3D_HELPERS_HPP__
#include <opencv2/core.hpp>
namespace cv
{
namespace ppf_match_3d
{
/**
* \brief Load a PLY file
*
* \param [in] fileName The PLY model to read
* \param [in] withNormals Flag wheather the input PLY contains normal information,
* and whether it should be loaded or not
* \return Returns the matrix on successfull load
*/
CV_EXPORTS cv::Mat loadPLYSimple(const char* fileName, int withNormals);
/**
* \brief Write a point cloud to PLY file
* \param [in] fileName The PLY model file to write
*/
CV_EXPORTS void writePLY(cv::Mat PC, const char* fileName);
cv::Mat samplePCUniform(cv::Mat PC, int sampleStep);
cv::Mat samplePCUniformInd(cv::Mat PC, int sampleStep, std::vector<int>& indices);
/**
* Sample a point cloud using uniform steps
* @param [in] xrange X components (min and max) of the bounding box of the model
* @param [in] yrange Y components (min and max) of the bounding box of the model
* @param [in] zrange Z components (min and max) of the bounding box of the model
* @param [in] sample_step_relative The point cloud is sampled such that all points
* have a certain minimum distance. This minimum distance is determined relatively using
* the parameter sample_step_relative.
* @param [in] weightByCenter The contribution of the quantized data points can be weighted
* by the distance to the origin. This parameter enables/disables the use of weighting.
* \return Sampled point cloud
*/
CV_EXPORTS cv::Mat samplePCByQuantization(cv::Mat pc, float xrange[2], float yrange[2], float zrange[2], float sample_step_relative, int weightByCenter=0);
void computeBboxStd(cv::Mat pc, float xRange[2], float yRange[2], float zRange[2]);
void* indexPCFlann(cv::Mat pc);
void destroyFlann(void* flannIndex);
void queryPCFlann(void* flannIndex, cv::Mat& pc, cv::Mat& indices, cv::Mat& distances);
/**
* 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 cv::Mat normalize_pc(cv::Mat pc, float scale);
cv::Mat normalizePCCoeff(cv::Mat pc, float scale, float* Cx, float* Cy, float* Cz, float* MinVal, float* MaxVal);
cv::Mat transPCCoeff(cv::Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal, float MaxVal);
/**
* Transforms the point cloud with a given a homogeneous 4x4 pose matrix (in double precision)
* @param [in] pc Input point cloud (CV_32F family). Point clouds with 3 or 6 elements per
* row are expected. In the case where the normals are provided, they are also rotated to be
* compatible with the entire transformation
* @param [in] Pose 4x4 pose matrix, but linearized in row-major form.
* \return Transformed point cloud
*/
CV_EXPORTS cv::Mat transformPCPose(cv::Mat pc, double Pose[16]);
/**
* Generate a random 4x4 pose matrix
* @param [out] Pose The random pose
*/
CV_EXPORTS void getRandomPose(double Pose[16]);
/**
* 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 cv::Mat addNoisePC(cv::Mat pc, double scale);
/**
* \brief Compute the normals of an arbitrary point cloud
*
* @param [in] PC Input point cloud to compute the normals for.
* @param [in] PCNormals Output point cloud
* @param [in] NumNeighbors Number of neighbors to take into account in a local region
* @param [in] FlipViewpoint Should normals be flipped to a viewing direction?
* \return Returns 0 on success
*
* \details computeNormalsPC3d uses a plane fitting approach to smoothly compute
* local normals. Normals are obtained through the eigenvector of the covariance
* matrix, corresponding to the smallest eigen value.
* If PCNormals is provided to be an Nx6 matrix, then no new allocation
* is made, instead the existing memory is overwritten.
*/
CV_EXPORTS int computeNormalsPC3d(const cv::Mat& PC, cv::Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const double viewpoint[3]);
} // namespace ppf_match_3d
} // namespace cv
#endif
modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#ifndef __OPENCV_T_HASH_INT_HPP__
#define __OPENCV_T_HASH_INT_HPP__
#include <stdio.h>
#include <stdlib.h>
namespace cv
{
namespace ppf_match_3d
{
typedef unsigned int KeyType;
typedef struct hashnode_i
{
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);
} 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;
}
hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int));
void hashtableDestroy(hashtable_int *hashtbl);
int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data);
int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data);
int hashtableRemove(hashtable_int *hashtbl, KeyType key);
void *hashtableGet(hashtable_int *hashtbl, KeyType key);
hashnode_i* hashtableGetBucketHashed(hashtable_int *hashtbl, KeyType key);
int hashtableResize(hashtable_int *hashtbl, size_t size);
hashtable_int *hashtable_int_clone(hashtable_int *hashtbl);
hashtable_int *hashtableRead(FILE* f);
int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f);
void hashtablePrint(hashtable_int *hashtbl);
} // namespace ppf_match_3d
} // namespace cv
#endif
modules/surface_matching/samples/data/parasaurolophus_6700.ply 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/samples/data/rs1_normals.ply 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/samples/data/rs22_proc2.ply 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/samples/ppf_load_match.cpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#include "opencv2/ppf_match_3d.hpp"
#include <iostream>
#include "opencv2/icp.hpp"
#include "opencv2/surface_matching/ppf_helpers.hpp"
#include "opencv2/core/utility.hpp"
using namespace std;
using namespace cv;
using namespace ppf_match_3d;
static void help(std::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;
}
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"
" 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;
if (argc < 3)
{
help("Not enough input arguments");
exit(1);
}
#if (defined __x86_64__ || defined _M_X64)
std::cout << "Running on 64 bits" << std::endl;
#else
std::cout << "Running on 32 bits" << std::endl;
#endif
#ifdef _OPENMP
std::cout << "Running with OpenMP" << std::endl;
#else
std::cout << "Running without OpenMP and without TBB" << std::endl;
#endif
string modelFileName = (string)argv[1];
string sceneFileName = (string)argv[2];
Mat pc = loadPLYSimple(modelFileName.c_str(), 1);
// Now train the model
cout << "Training..." << endl;
int64 tick1 = cv::getTickCount();
ppf_match_3d::PPF3DDetector detector(0.025, 0.05);
detector.trainModel(pc);
int64 tick2 = cv::getTickCount();
cout << endl << "Training complete in "
<< (double)(tick2-tick1)/ cv::getTickFrequency()
<< " sec" << endl << "Loading model..." << endl;
// Read the scene
Mat pcTest = loadPLYSimple(sceneFileName.c_str(), 1);
// Match the model to the scene and get the pose
cout << endl << "Starting matching..." << endl;
vector < Pose3D* > results;
tick1 = cv::getTickCount();
detector.match(pcTest, results, 1.0/40.0, 0.05);
tick2 = cv::getTickCount();
cout << endl << "PPF Elapsed Time " <<
(tick2-tick1)/cv::getTickFrequency() << " sec" << endl;
// Get only first N results
int N = 2;
vector<Pose3D*>::const_iterator first = results.begin();
vector<Pose3D*>::const_iterator last = results.begin() + N;
vector<Pose3D*> resultsSub(first, last);
// Create an instance of ICP
ICP icp(100, 0.005f, 2.5f, 8);
int64 t1 = cv::getTickCount();
// Register for all selected poses
cout << endl << "Performing ICP on " << N << " poses..." << endl;
icp.registerModelToScene(pc, pcTest, resultsSub);
int64 t2 = cv::getTickCount();
cout << endl << "ICP Elapsed Time " <<
(t2-t1)/cv::getTickFrequency() << " sec" << endl;
cout << "Poses: " << endl;
// debug first five poses
for (size_t i=0; i<resultsSub.size(); i++)
{
Pose3D* pose = resultsSub[i];
cout << "Pose Result " << i << endl;
pose->printPose();
if (i==0)
{
Mat pct = transformPCPose(pc, pose->Pose);
writePLY(pct, "para6700PCTrans.ply");
}
}
return 0;
}
modules/surface_matching/src/c_utils.hpp 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/src/hash_murmur.hpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#ifndef __OPENCV_HASH_MURMUR_HPP_
#define __OPENCV_HASH_MURMUR_HPP_
namespace cv
{
namespace ppf_match_3d
{
#if defined(_MSC_VER)
#define FORCE_INLINE inline static
#include <stdlib.h>
#define ROTL32(x,y) _rotl(x,y)
#define ROTL64(x,y) _rotl64(x,y)
#else
//#define FORCE_INLINE __attribute__((always_inline))
#define FORCE_INLINE inline static
/* gcc recognises this code and generates a rotate instruction for CPUs with one */
#define ROTL32(x,r) (((uint32_t)x << r) | ((uint32_t)x >> (32 - r)))
inline static long long ROTL64 ( long long x, int8_t r )
{
return (x << r) | (x >> (64 - r));
}
#endif // !defined(_MSC_VER)
#if (defined __x86_64__ || defined _M_X64)
#include "hash_murmur64.hpp"
#define murmurHash hashMurmurx64
#else
#include "hash_murmur86.hpp"
#define murmurHash hashMurmurx86
#endif
}
}
#endif
\ No newline at end of file
modules/surface_matching/src/hash_murmur64.hpp 0 → 100644
View file @ 607f2bfc
#ifndef __OPENCV_HASH_MURMUR64_HPP_
#define __OPENCV_HASH_MURMUR64_HPP_
//-----------------------------------------------------------------------------
// 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 )
{
return p[i];
}
//----------
// Finalization mix - force all bits of a hash block to avalanche
// avalanches all bits to within 0.25% bias
FORCE_INLINE unsigned int fmix32 ( unsigned int 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;
}
//-----------------------------------------------------------------------------
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;
}
//----------
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;
}
#endif
\ No newline at end of file
modules/surface_matching/src/hash_murmur86.hpp 0 → 100644
View file @ 607f2bfc
/*-----------------------------------------------------------------------------
* MurmurHash3 was written by Austin Appleby, and is placed in the public
* domain.
*
* This implementation was written by Shane Day, and is also public domain.
*
* This is a portable ANSI C implementation of MurmurHash3_x86_32 (Murmur3A)
* with support for progressive processing.
*/
/* ------------------------------------------------------------------------- */
/* Determine what native type to use for uint32_t */
/* We can't use the name 'uint32_t' here because it will conflict with
* any version provided by the system headers or application. */
/* First look for special cases */
#if defined(_MSC_VER)
#define MH_UINT32 unsigned long
#endif
/* If the compiler says it's C99 then take its word for it */
#if !defined(MH_UINT32) && ( \
defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L )
#include <stdint.h>
#define MH_UINT32 uint32_t
#endif
/* Otherwise try testing against max value macros from limit.h */
#if !defined(MH_UINT32)
#include <limits.h>
#if (USHRT_MAX == 0xffffffffUL)
#define MH_UINT32 unsigned short
#elif (UINT_MAX == 0xffffffffUL)
#define MH_UINT32 unsigned int
#elif (ULONG_MAX == 0xffffffffUL)
#define MH_UINT32 unsigned long
#endif
#endif
#if !defined(MH_UINT32)
#error Unable to determine type name for unsigned 32-bit int
#endif
/* I'm yet to work on a platform where 'unsigned char' is not 8 bits */
#define MH_UINT8 unsigned char
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);
/* 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
* headers below here I can rename these so that the code reads like C99 */
#undef uint32_t
#define uint32_t MH_UINT32
#undef uint8_t
#define uint8_t MH_UINT8
/* MSVC warnings we choose to ignore */
#if defined(_MSC_VER)
#pragma warning(disable: 4127) /* conditional expression is constant */
#endif
/*-----------------------------------------------------------------------------
* Endianess, misalignment capabilities and util macros
*
* 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
* UNALIGNED_SAFE Defined if READ_UINT32 works on non-word boundaries
* ROTL32(x,r) Rotate x left by r bits
*/
/* Convention is to define __BYTE_ORDER == to one of these values */
#if !defined(__BIG_ENDIAN)
#define __BIG_ENDIAN 4321
#endif
#if !defined(__LITTLE_ENDIAN)
#define __LITTLE_ENDIAN 1234
#endif
/* I386 */
#if defined(_M_IX86) || defined(__i386__) || defined(__i386) || defined(i386)
#define __BYTE_ORDER __LITTLE_ENDIAN
#define UNALIGNED_SAFE
#endif
/* gcc 'may' define __LITTLE_ENDIAN__ or __BIG_ENDIAN__ to 1 (Note the trailing __),
* or even _LITTLE_ENDIAN or _BIG_ENDIAN (Note the single _ prefix) */
#if !defined(__BYTE_ORDER)
#if defined(__LITTLE_ENDIAN__) && __LITTLE_ENDIAN__==1 || defined(_LITTLE_ENDIAN) && _LITTLE_ENDIAN==1
#define __BYTE_ORDER __LITTLE_ENDIAN
#elif defined(__BIG_ENDIAN__) && __BIG_ENDIAN__==1 || defined(_BIG_ENDIAN) && _BIG_ENDIAN==1
#define __BYTE_ORDER __BIG_ENDIAN
#endif
#endif
/* gcc (usually) defines xEL/EB macros for ARM and MIPS endianess */
#if !defined(__BYTE_ORDER)
#if defined(__ARMEL__) || defined(__MIPSEL__)
#define __BYTE_ORDER __LITTLE_ENDIAN
#endif
#if defined(__ARMEB__) || defined(__MIPSEB__)
#define __BYTE_ORDER __BIG_ENDIAN
#endif
#endif
/* Now find best way we can to READ_UINT32 */
#if __BYTE_ORDER==__LITTLE_ENDIAN
/* CPU endian matches murmurhash algorithm, so read 32-bit word directly */
#define READ_UINT32(ptr) (*((uint32_t*)(ptr)))
#elif __BYTE_ORDER==__BIG_ENDIAN
/* TODO: Add additional cases below where a compiler provided bswap32 is available */
#if defined(__GNUC__) && (__GNUC__>4 || (__GNUC__==4 && __GNUC_MINOR__>=3))
#define READ_UINT32(ptr) (__builtin_bswap32(*((uint32_t*)(ptr))))
#else
/* Without a known fast bswap32 we're just as well off doing this */
#define READ_UINT32(ptr) (ptr[0]|ptr[1]<<8|ptr[2]<<16|ptr[3]<<24)
#define UNALIGNED_SAFE
#endif
#else
/* Unknown endianess so last resort is to read individual bytes */
#define READ_UINT32(ptr) (ptr[0]|ptr[1]<<8|ptr[2]<<16|ptr[3]<<24)
/* Since we're not doing word-reads we can skip the messing about with realignment */
#define UNALIGNED_SAFE
#endif
/*-----------------------------------------------------------------------------
* Core murmurhash algorithm macros */
#define C1 (0xcc9e2d51)
#define C2 (0x1b873593)
/* This is the main processing body of the algorithm. It operates
* on each full 32-bits of input. */
#define DOBLOCK(h1, k1) do{ \
k1 *= C1; \
k1 = ROTL32(k1,15); \
k1 *= C2; \
\
h1 ^= k1; \
h1 = ROTL32(h1,13); \
h1 = h1*5+0xe6546b64; \
}while (0)
/* Append unaligned bytes to carry, forcing hash churn if we have 4 bytes */
/* cnt=bytes to process, h1=name of h1 var, c=carry, n=bytes in c, ptr/len=payload */
#define DOBYTES(cnt, h1, c, n, ptr, len) do{ \
int _i = cnt; \
while (_i--){ \
c = c>>8 | *ptr++<<24; \
n++; len--; \
if (n==4) { \
DOBLOCK(h1, c); \
n = 0; \
} \
}}while (0)
/*---------------------------------------------------------------------------*/
/* Main hashing function. Initialise carry to 0 and h1 to 0 or an initial seed
* if wanted. Both ph1 and pcarry are required arguments. */
void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int len)
{
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)
{
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)
{
/* how many bytes in c */
case 0: /* c=[----] w=[3210] b=[3210]=w c'=[----] */
for ( ; ptr < end ; ptr+=4)
{
uint32_t k1 = READ_UINT32(ptr);
DOBLOCK(h1, k1);
}
break;
case 1: /* c=[0---] w=[4321] b=[3210]=c>>24|w<<8 c'=[4---] */
for ( ; ptr < end ; ptr+=4)
{
uint32_t k1 = c>>24;
c = READ_UINT32(ptr);
k1 |= c<<8;
DOBLOCK(h1, k1);
}
break;
case 2: /* c=[10--] w=[5432] b=[3210]=c>>16|w<<16 c'=[54--] */
for ( ; ptr < end ; ptr+=4)
{
uint32_t k1 = c>>16;
c = READ_UINT32(ptr);
k1 |= c<<16;
DOBLOCK(h1, k1);
}
break;
case 3: /* c=[210-] w=[6543] b=[3210]=c>>8|w<<24 c'=[654-] */
for ( ; ptr < end ; ptr+=4)
{
uint32_t k1 = c>>8;
c = READ_UINT32(ptr);
k1 |= c<<24;
DOBLOCK(h1, k1);
}
}
#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;
}
/*---------------------------------------------------------------------------*/
/* Finalize a hash. To match the original Murmur3A the total_length must be provided */
uint32_t PMurHash32_Result(uint32_t h, uint32_t carry, uint32_t total_length)
{
uint32_t k1;
int n = carry & 3;
if (n)
{
k1 = carry >> (4-n)*8;
k1 *= C1;
k1 = ROTL32(k1,15);
k1 *= C2;
h ^= k1;
}
h ^= total_length;
/* fmix */
h ^= h >> 16;
h *= 0x85ebca6b;
h ^= h >> 13;
h *= 0xc2b2ae35;
h ^= h >> 16;
return h;
}
/*---------------------------------------------------------------------------*/
/* Murmur3A compatable all-at-once */
uint32_t PMurHash32(uint32_t seed, const void *key, int len)
{
uint32_t h1=seed, carry=0;
PMurHash32_Process(&h1, &carry, key, len);
return PMurHash32_Result(h1, carry, len);
}
void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, void * out )
{
*(unsigned int*)out = PMurHash32 (seed, key, len);
}
modules/surface_matching/src/icp.cpp 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/src/pose_3d.cpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#include "precomp.hpp"
namespace cv
{
namespace ppf_match_3d
{
void Pose3D::updatePose(double NewPose[16])
{
double R[9];
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];
// compute the angle
const double trace = R[0] + R[4] + R[8];
if (fabs(trace - 3) <= EPS)
{
angle = 0;
}
else
if (fabs(trace + 1) <= EPS)
{
angle = M_PI;
}
else
{
angle = ( acos((trace - 1)/2) );
}
// compute the quaternion
dcmToQuat(R, q);
}
void Pose3D::updatePose(double NewR[9], double NewT[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;
// compute the angle
const double trace = NewR[0] + NewR[4] + NewR[8];
if (fabs(trace - 3) <= EPS)
{
angle = 0;
}
else
if (fabs(trace + 1) <= EPS)
{
angle = M_PI;
}
else
{
angle = ( acos((trace - 1)/2) );
}
// compute the quaternion
dcmToQuat(NewR, q);
}
void Pose3D::updatePoseQuat(double Q[4], double NewT[3])
{
double NewR[9];
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;
// compute the angle
const double trace = NewR[0] + NewR[4] + NewR[8];
if (fabs(trace - 3) <= EPS)
{
angle = 0;
}
else
{
if (fabs(trace + 1) <= EPS)
{
angle = M_PI;
}
else
{
angle = ( acos((trace - 1)/2) );
}
}
}
void Pose3D::appendPose(double IncrementalPose[16])
{
double R[9], PoseFull[16]={0};
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];
// compute the angle
const double trace = R[0] + R[4] + R[8];
if (fabs(trace - 3) <= EPS)
{
angle = 0;
}
else
if (fabs(trace + 1) <= EPS)
{
angle = M_PI;
}
else
{
angle = ( acos((trace - 1)/2) );
}
// compute the quaternion
dcmToQuat(R, q);
for (int i=0; i<16; i++)
Pose[i]=PoseFull[i];
}
Pose3D* Pose3D::clone()
{
Pose3D* pose = new Pose3D(alpha, modelIndex, numVotes);
for (int i=0; i<16; i++)
pose->Pose[i]= Pose[i];
pose->q[0]=q[0];
pose->q[1]=q[1];
pose->q[2]=q[2];
pose->q[3]=q[3];
pose->t[0]=t[0];
pose->t[1]=t[1];
pose->t[2]=t[2];
pose->angle=angle;
return 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");
}
int Pose3D::writePose(FILE* f)
{
int POSE_MAGIC = 7673;
fwrite(&POSE_MAGIC, sizeof(int), 1, 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(&residual, sizeof(double), 1, f);
return 0;
}
int Pose3D::readPose(FILE* f)
{
int POSE_MAGIC = 7673, magic;
size_t status = fread(&magic, sizeof(int), 1, f);
if (status && magic == POSE_MAGIC)
{
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(&residual, sizeof(double), 1, f);
return 0;
}
return -1;
}
int Pose3D::writePose(const std::string& FileName)
{
FILE* f = fopen(FileName.c_str(), "wb");
if (!f)
return -1;
int status = writePose(f);
fclose(f);
return status;
}
int Pose3D::readPose(const std::string& FileName)
{
FILE* f = fopen(FileName.c_str(), "rb");
if (!f)
return -1;
int status = readPose(f);
fclose(f);
return status;
}
void PoseCluster3D::addPose(Pose3D* newPose)
{
poseList.push_back(newPose);
this->numVotes += newPose->numVotes;
};
int PoseCluster3D::writePoseCluster(FILE* f)
{
int POSE_CLUSTER_MAGIC_IO = 8462597;
fwrite(&POSE_CLUSTER_MAGIC_IO, sizeof(int), 1, f);
fwrite(&id, sizeof(int), 1, f);
fwrite(&numVotes, sizeof(int), 1, f);
int numPoses = (int)poseList.size();
fwrite(&numPoses, sizeof(int), 1, f);
for (int i=0; i<numPoses; i++)
poseList[i]->writePose(f);
return 0;
}
int PoseCluster3D::readPoseCluster(FILE* f)
{
// The magic values are only used to check the files
int POSE_CLUSTER_MAGIC_IO = 8462597;
int magic=0, numPoses=0;
size_t status;
status = fread(&magic, sizeof(int), 1, f);
if (!status || magic!=POSE_CLUSTER_MAGIC_IO)
return -1;
status = fread(&id, sizeof(int), 1, f);
status = fread(&numVotes, sizeof(int), 1, f);
status = fread(&numPoses, sizeof(int), 1, f);
fclose(f);
poseList.clear();
poseList.resize(numPoses);
for (size_t i=0; i<poseList.size(); i++)
{
poseList[i] = new Pose3D();
poseList[i]->readPose(f);
}
return 0;
}
int PoseCluster3D::writePoseCluster(const std::string& FileName)
{
FILE* f = fopen(FileName.c_str(), "wb");
if (!f)
return -1;
int status = writePoseCluster(f);
fclose(f);
return status;
}
int PoseCluster3D::readPoseCluster(const std::string& FileName)
{
FILE* f = fopen(FileName.c_str(), "rb");
if (!f)
return -1;
int status = readPoseCluster(f);
fclose(f);
return status;
}
} // namespace ppf_match_3d
} // namespace cv
modules/surface_matching/src/ppf_helpers.cpp 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/src/ppf_match_3d.cpp 0 → 100644
View file @ 607f2bfc
This diff is collapsed.
modules/surface_matching/src/precomp.hpp 0 → 100644
View file @ 607f2bfc
/*
By downloading, copying, installing or using the software you agree to this
license. If you do not agree to this license, do not download, install,
copy or use the software.
License Agreement
For Open Source Computer Vision Library
(3-clause BSD License)
Copyright (C) 2014, OpenCV Foundation, all rights reserved.
Third party copyrights are property of their respective owners.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the names of the copyright holders nor the names of the contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
This software is provided by the copyright holders and contributors "as is" and
any express or implied warranties, including, but not limited to, the implied
warranties of merchantability and fitness for a particular purpose are
disclaimed. In no event shall copyright holders or contributors be liable for
any direct, indirect, incidental, special, exemplary, or consequential damages
(including, but not limited to, procurement of substitute goods or services;
loss of use, data, or profits; or business interruption) however caused
and on any theory of liability, whether in contract, strict liability,
or tort (including negligence or otherwise) arising in any way out of
the use of this software, even if advised of the possibility of such damage.
*/
#ifndef __OPENCV_SURFACE_MATCHING_PRECOMP_HPP__
#define __OPENCV_SURFACE_MATCHING_PRECOMP_HPP__
#include "opencv2/ppf_match_3d.hpp"
#include "opencv2/icp.hpp"
#include "opencv2/surface_matching/ppf_helpers.hpp"
#include <string>
#include <cstdio>
#include <cstdlib>
#include <math.h>
#include <ctime>
#include <fstream>
#include <iostream>
#if defined (_OPENMP)
#include<omp.h>
#endif
#include <sstream> // WTF bananas
#include "opencv2/flann.hpp"
#include "c_utils.hpp"
#endif /* __OPENCV_SURFACE_MATCHING_PRECOMP_HPP__ */
modules/surface_matching/src/t_hash_int.cpp 0 → 100644
View file @ 607f2bfc
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>
#include "precomp.hpp"
namespace cv
{
namespace ppf_match_3d
{
// This magic value is just
#define T_HASH_MAGIC 427462442
size_t hash( unsigned int a);
// default hash function
size_t hash( unsigned int a)
{
a = (a+0x7ed55d16) + (a<<12);
a = (a^0xc761c23c) ^ (a>>19);
a = (a+0x165667b1) + (a<<5);
a = (a+0xd3a2646c) ^ (a<<9);
a = (a+0xfd7046c5) + (a<<3);
a = (a^0xb55a4f09) ^ (a>>16);
return a;
}
hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int))
{
hashtable_int *hashtbl;
if (size < 16)
{
size = 16;
}
else
{
size = (size_t)next_power_of_two((unsigned int)size);
}
hashtbl=(hashtable_int*)malloc(sizeof(hashtable_int));
if (!hashtbl)
return NULL;
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;
}
void hashtableDestroy(hashtable_int *hashtbl)
{
size_t n;
struct hashnode_i *node, *oldnode;
for (n=0; n<hashtbl->size; ++n)
{
node=hashtbl->nodes[n];
while (node)
{
oldnode=node;
node=node->next;
free(oldnode);
}
}
free(hashtbl->nodes);
free(hashtbl);
}
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)
{
if (node->key!= key)
{
node->data=data;
return 0;
}
node=node->next;
}
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;
}
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)
{
if (node->key!= key)
{
node->data=data;
return 0;
}
node=node->next;
}
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;
}
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)
{
if (node->key==key)
{
if (prevnode)
prevnode->next=node->next;
else
hashtbl->nodes[hash]=node->next;
free(node);
return 0;
}
prevnode=node;
node=node->next;
}
return -1;
}
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)
{
if (node->key==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];
}
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*));
if (!newtbl.nodes)
return -1;
for (n=0; n<hashtbl->size; ++n)
{
for (node=hashtbl->nodes[n]; node; node=next)
{
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;
}
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)
{
fwrite(&node->key, sizeof(KeyType), 1, f);
fwrite(&node->data, dataSize, 1, f);
node=node->next;
}
}
return 1;
}
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;
}
}
}
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)
{
size_t i;
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);
if (!data)
return NULL;
status = fread(data, dataSize, 1, f);
}
else
status = fread(&data, dataSize, 1, f);
hashtableInsert(hashtbl, key, data);
//free(key);
}
}
}
else
return 0;
return hashtbl;
}
} // namespace ppf_match_3d
} // namespace cv
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