Skip to content

O
opencv_contrib
Commit 3f620fd1 authored by Bence Magyar's avatar Bence Magyar
Browse files
Options

Refactored API classes, removed unused params

parent 995b6963
Hide whitespace changes
Inline Side-by-side
Showing with 129 additions and 138 deletions
modules/surface_matching/include/opencv2/surface_matching/icp.hpp
View file @ 3f620fd1
...@@ -89,9 +89,9 @@ public: ...@@ -89,9 +89,9 @@ public:
ICP() ICP()
{ {
m_tolerence = 0.005f; m_tolerance = 0.005f;
m_rejectionScale = 2.5f; m_rejectionScale = 2.5f;
m_maxItereations = 250; m_maxIterations = 250;
m_numLevels = 6; m_numLevels = 6;
m_sampleType = ICP_SAMPLING_TYPE_UNIFORM; m_sampleType = ICP_SAMPLING_TYPE_UNIFORM;
m_numNeighborsCorr = 1; m_numNeighborsCorr = 1;
...@@ -112,10 +112,10 @@ public: ...@@ -112,10 +112,10 @@ public:
*/ */
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) 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_tolerance = tolerence;
m_numNeighborsCorr = numMaxCorr; m_numNeighborsCorr = numMaxCorr;
m_rejectionScale = rejectionScale; m_rejectionScale = rejectionScale;
m_maxItereations = iterations; m_maxIterations = iterations;
m_numLevels = numLevels; m_numLevels = numLevels;
m_sampleType = sampleType; m_sampleType = sampleType;
} }
...@@ -147,8 +147,8 @@ public: ...@@ -147,8 +147,8 @@ public:
int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses); int registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3D*>& poses);
private: private:
float m_tolerence; float m_tolerance;
int m_maxItereations; int m_maxIterations;
float m_rejectionScale; float m_rejectionScale;
int m_numNeighborsCorr; int m_numNeighborsCorr;
int m_numLevels; int m_numLevels;
......
modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp
View file @ 3f620fd1
...@@ -66,7 +66,7 @@ public: ...@@ -66,7 +66,7 @@ public:
residual = 0; residual = 0;
for (int i=0; i<16; i++) for (int i=0; i<16; i++)
Pose[i]=0; pose[i]=0;
} }
Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0) Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0)
...@@ -77,7 +77,7 @@ public: ...@@ -77,7 +77,7 @@ public:
residual=0; residual=0;
for (int i=0; i<16; i++) for (int i=0; i<16; i++)
Pose[i]=0; pose[i]=0;
} }
/** /**
...@@ -117,7 +117,7 @@ public: ...@@ -117,7 +117,7 @@ public:
double alpha, residual; double alpha, residual;
unsigned int modelIndex; unsigned int modelIndex;
unsigned int numVotes; unsigned int numVotes;
double Pose[16], angle, t[3], q[4]; double pose[16], angle, t[3], q[4];
}; };
/** /**
......
modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp
View file @ 3f620fd1
...@@ -67,16 +67,14 @@ namespace cv ...@@ -67,16 +67,14 @@ namespace cv
namespace ppf_match_3d namespace ppf_match_3d
{ {
#define T_PPF_LENGTH 5
/** /**
* @struct THash * @struct THash
* @brief Struct, holding a node in the hashtable * @brief Struct, holding a node in the hashtable
*/ */
typedef struct THash typedef struct THash
{ {
int id; int id;
int i, ppfInd; int i, ppfInd;
} THash; } THash;
/** /**
...@@ -113,13 +111,11 @@ public: ...@@ -113,13 +111,11 @@ public:
/** /**
* Set the parameters for the search * 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] 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] 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. * @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); void setSearchParams(const double positionThreshold=-1, const double rotationThreshold=-1, const bool useWeightedClustering=false);
/** /**
* \brief Trains a new model. * \brief Trains a new model.
...@@ -145,19 +141,17 @@ public: ...@@ -145,19 +141,17 @@ public:
protected: protected:
double maxDist, angle_step, angleStepRadians, distance_step; double angle_step, angle_step_radians, distance_step;
double samplingStepRelative, angleStepRelative, distanceStepRelative; double sampling_step_relative, angle_step_relative, distance_step_relative;
Mat inputPC, sampledPC, PPF; Mat sampled_pc, ppf;
int num_ref_points, sampled_step, ppf_step; int num_ref_points, ppf_step;
hashtable_int* hash_table; hashtable_int* hash_table;
THash* hash_nodes; THash* hash_nodes;
int NumPoses; double position_threshold, rotation_threshold;
double PositionThreshold, RotationThreshold, MinMatchScore; bool use_weighted_avg;
bool UseWeightedAvg;
float sampleStepSearch; int scene_sample_step;
int SceneSampleStep;
void clearTrainingModels(); void clearTrainingModels();
......
modules/surface_matching/samples/ppf_load_match.cpp
View file @ 3f620fd1
...@@ -133,12 +133,12 @@ int main(int argc, char** argv) ...@@ -133,12 +133,12 @@ int main(int argc, char** argv)
// debug first five poses // debug first five poses
for (size_t i=0; i<resultsSub.size(); i++) for (size_t i=0; i<resultsSub.size(); i++)
{ {
Pose3D* pose = resultsSub[i]; Pose3D* result = resultsSub[i];
cout << "Pose Result " << i << endl; cout << "Pose Result " << i << endl;
pose->printPose(); result->printPose();
if (i==0) if (i==0)
{ {
Mat pct = transformPCPose(pc, pose->Pose); Mat pct = transformPCPose(pc, result->pose);
writePLY(pct, "para6700PCTrans.ply"); writePLY(pct, "para6700PCTrans.ply");
} }
} }
......
modules/surface_matching/src/icp.cpp
View file @ 3f620fd1
...@@ -338,8 +338,8 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu ...@@ -338,8 +338,8 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& Residu
double div = pow((double)impact, (double)level); double div = pow((double)impact, (double)level);
//double div2 = div*div; //double div2 = div*div;
const int numSamples = cvRound((double)(n/(div))); const int numSamples = cvRound((double)(n/(div)));
const double TolP = m_tolerence*(double)(level+1)*(level+1); const double TolP = m_tolerance*(double)(level+1)*(level+1);
const int MaxIterationsPyr = cvRound((double)m_maxItereations/(level+1)); const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1));
// Obtain the sampled point clouds for this level: Also rotates the normals // Obtain the sampled point clouds for this level: Also rotates the normals
Mat srcPCT = transformPCPose(srcPC0, Pose); Mat srcPCT = transformPCPose(srcPC0, Pose);
...@@ -546,10 +546,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po ...@@ -546,10 +546,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po
{ {
for (size_t i=0; i<poses.size(); i++) for (size_t i=0; i<poses.size(); i++)
{ {
double PoseICP[16]={0}; double poseICP[16]={0};
Mat srcTemp = transformPCPose(srcPC, poses[i]->Pose); Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
registerModelToScene(srcTemp, dstPC, poses[i]->residual, PoseICP); registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
poses[i]->appendPose(PoseICP); poses[i]->appendPose(poseICP);
} }
return 0; return 0;
......
modules/surface_matching/src/pose_3d.cpp
View file @ 3f620fd1
...@@ -50,21 +50,21 @@ void Pose3D::updatePose(double NewPose[16]) ...@@ -50,21 +50,21 @@ void Pose3D::updatePose(double NewPose[16])
double R[9]; double R[9];
for (int i=0; i<16; i++) for (int i=0; i<16; i++)
Pose[i]=NewPose[i]; pose[i]=NewPose[i];
R[0] = Pose[0]; R[0] = pose[0];
R[1] = Pose[1]; R[1] = pose[1];
R[2] = Pose[2]; R[2] = pose[2];
R[3] = Pose[4]; R[3] = pose[4];
R[4] = Pose[5]; R[4] = pose[5];
R[5] = Pose[6]; R[5] = pose[6];
R[6] = Pose[8]; R[6] = pose[8];
R[7] = Pose[9]; R[7] = pose[9];
R[8] = Pose[10]; R[8] = pose[10];
t[0]=Pose[3]; t[0]=pose[3];
t[1]=Pose[7]; t[1]=pose[7];
t[2]=Pose[11]; t[2]=pose[11];
// compute the angle // compute the angle
const double trace = R[0] + R[4] + R[8]; const double trace = R[0] + R[4] + R[8];
...@@ -89,22 +89,22 @@ void Pose3D::updatePose(double NewPose[16]) ...@@ -89,22 +89,22 @@ void Pose3D::updatePose(double NewPose[16])
void Pose3D::updatePose(double NewR[9], double NewT[3]) void Pose3D::updatePose(double NewR[9], double NewT[3])
{ {
Pose[0]=NewR[0]; pose[0]=NewR[0];
Pose[1]=NewR[1]; pose[1]=NewR[1];
Pose[2]=NewR[2]; pose[2]=NewR[2];
Pose[3]=NewT[0]; pose[3]=NewT[0];
Pose[4]=NewR[3]; pose[4]=NewR[3];
Pose[5]=NewR[4]; pose[5]=NewR[4];
Pose[6]=NewR[5]; pose[6]=NewR[5];
Pose[7]=NewT[1]; pose[7]=NewT[1];
Pose[8]=NewR[6]; pose[8]=NewR[6];
Pose[9]=NewR[7]; pose[9]=NewR[7];
Pose[10]=NewR[8]; pose[10]=NewR[8];
Pose[11]=NewT[2]; pose[11]=NewT[2];
Pose[12]=0; pose[12]=0;
Pose[13]=0; pose[13]=0;
Pose[14]=0; pose[14]=0;
Pose[15]=1; pose[15]=1;
// compute the angle // compute the angle
const double trace = NewR[0] + NewR[4] + NewR[8]; const double trace = NewR[0] + NewR[4] + NewR[8];
...@@ -137,22 +137,22 @@ void Pose3D::updatePoseQuat(double Q[4], double NewT[3]) ...@@ -137,22 +137,22 @@ void Pose3D::updatePoseQuat(double Q[4], double NewT[3])
q[2]=Q[2]; q[2]=Q[2];
q[3]=Q[3]; q[3]=Q[3];
Pose[0]=NewR[0]; pose[0]=NewR[0];
Pose[1]=NewR[1]; pose[1]=NewR[1];
Pose[2]=NewR[2]; pose[2]=NewR[2];
Pose[3]=NewT[0]; pose[3]=NewT[0];
Pose[4]=NewR[3]; pose[4]=NewR[3];
Pose[5]=NewR[4]; pose[5]=NewR[4];
Pose[6]=NewR[5]; pose[6]=NewR[5];
Pose[7]=NewT[1]; pose[7]=NewT[1];
Pose[8]=NewR[6]; pose[8]=NewR[6];
Pose[9]=NewR[7]; pose[9]=NewR[7];
Pose[10]=NewR[8]; pose[10]=NewR[8];
Pose[11]=NewT[2]; pose[11]=NewT[2];
Pose[12]=0; pose[12]=0;
Pose[13]=0; pose[13]=0;
Pose[14]=0; pose[14]=0;
Pose[15]=1; pose[15]=1;
// compute the angle // compute the angle
const double trace = NewR[0] + NewR[4] + NewR[8]; const double trace = NewR[0] + NewR[4] + NewR[8];
...@@ -179,7 +179,7 @@ void Pose3D::appendPose(double IncrementalPose[16]) ...@@ -179,7 +179,7 @@ void Pose3D::appendPose(double IncrementalPose[16])
{ {
double R[9], PoseFull[16]={0}; double R[9], PoseFull[16]={0};
matrixProduct44(IncrementalPose, this->Pose, PoseFull); matrixProduct44(IncrementalPose, this->pose, PoseFull);
R[0] = PoseFull[0]; R[0] = PoseFull[0];
R[1] = PoseFull[1]; R[1] = PoseFull[1];
...@@ -216,27 +216,27 @@ void Pose3D::appendPose(double IncrementalPose[16]) ...@@ -216,27 +216,27 @@ void Pose3D::appendPose(double IncrementalPose[16])
dcmToQuat(R, q); dcmToQuat(R, q);
for (int i=0; i<16; i++) for (int i=0; i<16; i++)
Pose[i]=PoseFull[i]; pose[i]=PoseFull[i];
} }
Pose3D* Pose3D::clone() Pose3D* Pose3D::clone()
{ {
Pose3D* pose = new Pose3D(alpha, modelIndex, numVotes); Pose3D* new_pose = new Pose3D(alpha, modelIndex, numVotes);
for (int i=0; i<16; i++) for (int i=0; i<16; i++)
pose->Pose[i]= Pose[i]; new_pose->pose[i]= this->pose[i];
pose->q[0]=q[0]; new_pose->q[0]=q[0];
pose->q[1]=q[1]; new_pose->q[1]=q[1];
pose->q[2]=q[2]; new_pose->q[2]=q[2];
pose->q[3]=q[3]; new_pose->q[3]=q[3];
pose->t[0]=t[0]; new_pose->t[0]=t[0];
pose->t[1]=t[1]; new_pose->t[1]=t[1];
pose->t[2]=t[2]; new_pose->t[2]=t[2];
pose->angle=angle; new_pose->angle=angle;
return pose; return new_pose;
} }
void Pose3D::printPose() void Pose3D::printPose()
...@@ -246,7 +246,7 @@ void Pose3D::printPose() ...@@ -246,7 +246,7 @@ void Pose3D::printPose()
{ {
for (int k=0; k<4; k++) for (int k=0; k<4; k++)
{ {
printf("%f ", this->Pose[j*4+k]); printf("%f ", this->pose[j*4+k]);
} }
printf("\n"); printf("\n");
} }
...@@ -260,7 +260,7 @@ int Pose3D::writePose(FILE* f) ...@@ -260,7 +260,7 @@ int Pose3D::writePose(FILE* f)
fwrite(&angle, sizeof(double), 1, f); fwrite(&angle, sizeof(double), 1, f);
fwrite(&numVotes, sizeof(int), 1, f); fwrite(&numVotes, sizeof(int), 1, f);
fwrite(&modelIndex, sizeof(int), 1, f); fwrite(&modelIndex, sizeof(int), 1, f);
fwrite(Pose, sizeof(double)*16, 1, f); fwrite(pose, sizeof(double)*16, 1, f);
fwrite(t, sizeof(double)*3, 1, f); fwrite(t, sizeof(double)*3, 1, f);
fwrite(q, sizeof(double)*4, 1, f); fwrite(q, sizeof(double)*4, 1, f);
fwrite(&residual, sizeof(double), 1, f); fwrite(&residual, sizeof(double), 1, f);
...@@ -277,7 +277,7 @@ int Pose3D::readPose(FILE* f) ...@@ -277,7 +277,7 @@ int Pose3D::readPose(FILE* f)
status = fread(&angle, sizeof(double), 1, f); status = fread(&angle, sizeof(double), 1, f);
status = fread(&numVotes, sizeof(int), 1, f); status = fread(&numVotes, sizeof(int), 1, f);
status = fread(&modelIndex, sizeof(int), 1, f); status = fread(&modelIndex, sizeof(int), 1, f);
status = fread(Pose, sizeof(double)*16, 1, f); status = fread(pose, sizeof(double)*16, 1, f);
status = fread(t, sizeof(double)*3, 1, f); status = fread(t, sizeof(double)*3, 1, f);
status = fread(q, sizeof(double)*4, 1, f); status = fread(q, sizeof(double)*4, 1, f);
status = fread(&residual, sizeof(double), 1, f); status = fread(&residual, sizeof(double), 1, f);
......
modules/surface_matching/src/ppf_match_3d.cpp
View file @ 3f620fd1
...@@ -45,6 +45,9 @@ namespace cv ...@@ -45,6 +45,9 @@ namespace cv
{ {
namespace ppf_match_3d namespace ppf_match_3d
{ {
static const size_t PPF_LENGTH = 5;
// routines for assisting sort // routines for assisting sort
static int qsortPoseCmp(const void * a, const void * b) static int qsortPoseCmp(const void * a, const void * b)
{ {
...@@ -118,12 +121,12 @@ static double computeAlpha(const double p1[4], const double n1[4], const double ...@@ -118,12 +121,12 @@ static double computeAlpha(const double p1[4], const double n1[4], const double
PPF3DDetector::PPF3DDetector() PPF3DDetector::PPF3DDetector()
{ {
samplingStepRelative = 0.05; sampling_step_relative = 0.05;
distanceStepRelative = 0.05; distance_step_relative = 0.05;
SceneSampleStep = (int)(1/0.04); scene_sample_step = (int)(1/0.04);
angleStepRelative = 30; angle_step_relative = 30;
angleStepRadians = (360.0/angleStepRelative)*M_PI/180.0; angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
angle_step = angleStepRadians; angle_step = angle_step_radians;
trained = false; trained = false;
setSearchParams(); setSearchParams();
...@@ -131,33 +134,30 @@ PPF3DDetector::PPF3DDetector() ...@@ -131,33 +134,30 @@ PPF3DDetector::PPF3DDetector()
PPF3DDetector::PPF3DDetector(const double RelativeSamplingStep, const double RelativeDistanceStep, const double NumAngles) PPF3DDetector::PPF3DDetector(const double RelativeSamplingStep, const double RelativeDistanceStep, const double NumAngles)
{ {
samplingStepRelative = RelativeSamplingStep; sampling_step_relative = RelativeSamplingStep;
distanceStepRelative = RelativeDistanceStep; distance_step_relative = RelativeDistanceStep;
angleStepRelative = NumAngles; angle_step_relative = NumAngles;
angleStepRadians = (360.0/angleStepRelative)*M_PI/180.0; angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
//SceneSampleStep = 1.0/RelativeSceneSampleStep; //SceneSampleStep = 1.0/RelativeSceneSampleStep;
angle_step = angleStepRadians; angle_step = angle_step_radians;
trained = false; trained = false;
setSearchParams(); setSearchParams();
} }
void PPF3DDetector::setSearchParams(const int numPoses, const double positionThreshold, const double rotationThreshold, const double minMatchScore, const bool useWeightedClustering) void PPF3DDetector::setSearchParams(const double positionThreshold, const double rotationThreshold, const bool useWeightedClustering)
{ {
NumPoses=numPoses;
if (positionThreshold<0) if (positionThreshold<0)
PositionThreshold = samplingStepRelative; position_threshold = sampling_step_relative;
else else
PositionThreshold = positionThreshold; position_threshold = positionThreshold;
if (rotationThreshold<0) if (rotationThreshold<0)
RotationThreshold = ((360/angle_step) / 180.0 * M_PI); rotation_threshold = ((360/angle_step) / 180.0 * M_PI);
else else
RotationThreshold = rotationThreshold; rotation_threshold = rotationThreshold;
UseWeightedAvg = useWeightedClustering; use_weighted_avg = useWeightedClustering;
MinMatchScore = minMatchScore;
} }
// compute per point PPF as in paper // compute per point PPF as in paper
...@@ -242,17 +242,17 @@ void PPF3DDetector::trainModel(const Mat &PC) ...@@ -242,17 +242,17 @@ void PPF3DDetector::trainModel(const Mat &PC)
float dz = zRange[1] - zRange[0]; float dz = zRange[1] - zRange[0];
float diameter = sqrt ( dx * dx + dy * dy + dz * dz ); float diameter = sqrt ( dx * dx + dy * dy + dz * dz );
float distanceStep = (float)(diameter * samplingStepRelative); float distanceStep = (float)(diameter * sampling_step_relative);
Mat sampled = samplePCByQuantization(PC, xRange, yRange, zRange, (float)samplingStepRelative,0); Mat sampled = samplePCByQuantization(PC, xRange, yRange, zRange, (float)sampling_step_relative,0);
int size = sampled.rows*sampled.rows; int size = sampled.rows*sampled.rows;
hashtable_int* hashTable = hashtableCreate(size, NULL); hashtable_int* hashTable = hashtableCreate(size, NULL);
int numPPF = sampled.rows*sampled.rows; int numPPF = sampled.rows*sampled.rows;
PPF = Mat(numPPF, T_PPF_LENGTH, CV_32FC1); ppf = Mat(numPPF, PPF_LENGTH, CV_32FC1);
int ppfStep = (int)PPF.step; int ppfStep = (int)ppf.step;
int sampledStep = (int)sampled.step; int sampledStep = (int)sampled.step;
// TODO: Maybe I could sample 1/5th of them here. Check the performance later. // TODO: Maybe I could sample 1/5th of them here. Check the performance later.
...@@ -284,7 +284,7 @@ void PPF3DDetector::trainModel(const Mat &PC) ...@@ -284,7 +284,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
double f[4]={0}; double f[4]={0};
computePPFFeatures(p1, n1, p2, n2, f); computePPFFeatures(p1, n1, p2, n2, f);
KeyType hashValue = hashPPF(f, angleStepRadians, distanceStep); KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
double alpha = computeAlpha(p1, n1, p2); double alpha = computeAlpha(p1, n1, p2);
unsigned int corrInd = i*numRefPoints+j; unsigned int corrInd = i*numRefPoints+j;
unsigned int ppfInd = corrInd*ppfStep; unsigned int ppfInd = corrInd*ppfStep;
...@@ -296,7 +296,7 @@ void PPF3DDetector::trainModel(const Mat &PC) ...@@ -296,7 +296,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
hashtableInsertHashed(hashTable, hashValue, (void*)hashNode); hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
float* ppfRow = (float*)(&(PPF.data[ ppfInd ])); float* ppfRow = (float*)(&(ppf.data[ ppfInd ]));
ppfRow[0] = (float)f[0]; ppfRow[0] = (float)f[0];
ppfRow[1] = (float)f[1]; ppfRow[1] = (float)f[1];
ppfRow[2] = (float)f[2]; ppfRow[2] = (float)f[2];
...@@ -306,13 +306,12 @@ void PPF3DDetector::trainModel(const Mat &PC) ...@@ -306,13 +306,12 @@ void PPF3DDetector::trainModel(const Mat &PC)
} }
} }
angle_step = angleStepRadians; angle_step = angle_step_radians;
distance_step = distanceStep; distance_step = distanceStep;
hash_table = hashTable; hash_table = hashTable;
sampled_step = sampledStep;
ppf_step = ppfStep; ppf_step = ppfStep;
num_ref_points = numRefPoints; num_ref_points = numRefPoints;
sampledPC = sampled; sampled_pc = sampled;
trained = true; trained = true;
} }
...@@ -329,7 +328,7 @@ bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose ...@@ -329,7 +328,7 @@ bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose
const double phi = fabs ( sourcePose.angle - targetPose.angle ); const double phi = fabs ( sourcePose.angle - targetPose.angle );
return (phi<this->RotationThreshold && dNorm < this->PositionThreshold); return (phi<this->rotation_threshold && dNorm < this->position_threshold);
} }
int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pose3D*>& finalPoses) int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pose3D*>& finalPoses)
...@@ -371,7 +370,7 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos ...@@ -371,7 +370,7 @@ int PPF3DDetector::clusterPoses(Pose3D** poseList, int numPoses, std::vector<Pos
// TODO: Use MinMatchScore // TODO: Use MinMatchScore
if (UseWeightedAvg) if (use_weighted_avg)
{ {
#if defined _OPENMP #if defined _OPENMP
#pragma omp parallel for #pragma omp parallel for
...@@ -486,14 +485,14 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do ...@@ -486,14 +485,14 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
CV_Assert(pc.type() == CV_32F || pc.type() == CV_32FC1); CV_Assert(pc.type() == CV_32F || pc.type() == CV_32FC1);
CV_Assert(relativeSceneSampleStep<=1 && relativeSceneSampleStep>0); CV_Assert(relativeSceneSampleStep<=1 && relativeSceneSampleStep>0);
SceneSampleStep = (int)(1.0/relativeSceneSampleStep); scene_sample_step = (int)(1.0/relativeSceneSampleStep);
//int numNeighbors = 10; //int numNeighbors = 10;
int numAngles = (int) (floor (2 * M_PI / angle_step)); int numAngles = (int) (floor (2 * M_PI / angle_step));
float distanceStep = (float)distance_step; float distanceStep = (float)distance_step;
unsigned int n = num_ref_points; unsigned int n = num_ref_points;
Pose3D** poseList; Pose3D** poseList;
int sceneSamplingStep = SceneSampleStep; int sceneSamplingStep = scene_sample_step;
// compute bbox // compute bbox
float xRange[2], yRange[2], zRange[2]; float xRange[2], yRange[2], zRange[2];
...@@ -574,8 +573,8 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do ...@@ -574,8 +573,8 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
THash* tData = (THash*) node->data; THash* tData = (THash*) node->data;
int corrI = (int)tData->i; int corrI = (int)tData->i;
int ppfInd = (int)tData->ppfInd; int ppfInd = (int)tData->ppfInd;
float* ppfCorrScene = (float*)(&PPF.data[ppfInd]); float* ppfCorrScene = (float*)(&ppf.data[ppfInd]);
double alpha_model = (double)ppfCorrScene[T_PPF_LENGTH-1]; double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
double alpha = alpha_model - alpha_scene; double alpha = alpha_model - alpha_scene;
/* Tolga Birdal's note: Map alpha to the indices: /* Tolga Birdal's note: Map alpha to the indices:
...@@ -629,7 +628,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do ...@@ -629,7 +628,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
}; };
// TODO : Compute pose // TODO : Compute pose
const float* fMax = (float*)(&sampledPC.data[refIndMax * sampledPC.step]); const float* fMax = (float*)(&sampled_pc.data[refIndMax * sampled_pc.step]);
const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1}; const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1};
const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1}; const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1};
...@@ -656,11 +655,9 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do ...@@ -656,11 +655,9 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3D*>& results, const do
matrixProduct44(Talpha, Tmg, Temp); matrixProduct44(Talpha, Tmg, Temp);
matrixProduct44(TsgInv, Temp, Pose); matrixProduct44(TsgInv, Temp, Pose);
Pose3D *ppf = new Pose3D(alpha, refIndMax, maxVotes); Pose3D *pose = new Pose3D(alpha, refIndMax, maxVotes);
pose->updatePose(Pose);
ppf->updatePose(Pose); poseList[i/sceneSamplingStep] = pose;
poseList[i/sceneSamplingStep] = ppf;
#if defined (_OPENMP) #if defined (_OPENMP)
free(accumulator); free(accumulator);
......
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