int main(int argc, char **argv)
{
std::system("date");
if (argc != 4)
{
cout << argv[0] << " <modelsPath> <baseFoldler> <testObjectName>" << endl;
return -1;
}
string modelsPath = argv[1];
string baseFolder = argv[2];
string testObjectName = argv[3];
//const string modelFilename = "finalModels/" + objectName + ".xml";
const string testFolder = baseFolder + "/" + testObjectName + "/";
// const string camerasListFilename = baseFolder + "/cameras.txt";
const string kinectCameraFilename = baseFolder + "/center.yml";
// const string visualizationPath = "visualized_results/";
const string errorsVisualizationPath = "/home/ilysenkov/errors/";
// const vector<string> objectNames = {"bank", "bucket"};
// const vector<string> objectNames = {"bank", "bottle", "bucket", "glass", "wineglass"};
const string registrationMaskFilename = baseFolder + "/registrationMask.png";
const vector<string> objectNames = {testObjectName};
TODBaseImporter dataImporter(testFolder);
PinholeCamera kinectCamera;
if(!kinectCameraFilename.empty())
{
dataImporter.readCameraParams(kinectCameraFilename, kinectCamera, false);
CV_Assert(kinectCamera.imageSize == Size(640, 480));
}
vector<EdgeModel> edgeModels(objectNames.size());
for (size_t i = 0; i < objectNames.size(); ++i)
{
dataImporter.importEdgeModel(modelsPath, objectNames[i], edgeModels[i]);
cout << "All points in the model: " << edgeModels[i].points.size() << endl;
cout << "Surface points in the model: " << edgeModels[i].stableEdgels.size() << endl;
EdgeModel::computeSurfaceEdgelsOrientations(edgeModels[i]);
}
vector<EdgeModel> occlusionObjects;
vector<PoseRT> occlusionOffsets;
dataImporter.importOcclusionObjects(modelsPath, occlusionObjects, occlusionOffsets);
//#ifdef VISUALIZE_POSE_REFINEMENT
// edgeModels[0].visualize();
//#endif
DetectorParams params;
// params.glassSegmentationParams.closingIterations = 8;
// bucket
// params.glassSegmentationParams.openingIterations = 8;
//good clutter
params.glassSegmentationParams.openingIterations = 15;
params.glassSegmentationParams.closingIterations = 12;
params.glassSegmentationParams.finalClosingIterations = 32;
params.glassSegmentationParams.grabCutErosionsIterations = 4;
params.planeSegmentationMethod = FIDUCIALS;
//fixedOnTable
//params.glassSegmentationParams.finalClosingIterations = 8;
//clutter
//bucket
//params.glassSegmentationParams.finalClosingIterations = 12;
Detector detector(kinectCamera, params);
// TransparentDetector detector(kinectCamera);
for (size_t i = 0; i < edgeModels.size(); ++i)
{
detector.addTrainObject(objectNames[i], edgeModels[i]);
}
vector<int> testIndices;
dataImporter.importTestIndices(testIndices);
Mat registrationMask = imread(registrationMaskFilename, CV_LOAD_IMAGE_GRAYSCALE);
CV_Assert(!registrationMask.empty());
vector<size_t> initialPoseCount;
vector<PoseError> bestPoses, bestInitialPoses;
int segmentationFailuresCount = 0;
int badSegmentationCount = 0;
vector<float> allChamferDistances;
vector<size_t> geometricHashingPoseCount;
// vector<int> indicesOfRecognizedObjects;
vector<double> allRecognitionTimes;
for(size_t testIdx = 0; testIdx < testIndices.size(); testIdx++)
{
srand(42);
RNG &rng = theRNG();
rng.state = 0xffffffff;
#if defined(VISUALIZE_POSE_REFINEMENT) && defined(USE_3D_VISUALIZATION)
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer ("transparent experiments"));
#endif
int testImageIdx = testIndices[ testIdx ];
cout << "Test: " << testIdx << " " << testImageIdx << endl;
Mat kinectDepth, kinectBgrImage;
if(!kinectCameraFilename.empty())
{
dataImporter.importBGRImage(testImageIdx, kinectBgrImage);
dataImporter.importDepth(testImageIdx, kinectDepth);
}
/*
Mat silhouetteImage(480, 640, CV_8UC1, Scalar(0));
silhouettes[0].draw(silhouetteImage);
imshow("silhouette", silhouetteImage);
imshow("mask", centerMask);
waitKey();
vector<Point2f> glassContour;
mask2contour(centerMask, glassContour);
Mat silhouette2test;
silhouettes[0].match(Mat(glassContour), silhouette2test);
exit(-1);
*/
PoseRT model2test_fiducials, objectOffset;
dataImporter.importGroundTruth(testImageIdx, model2test_fiducials, false, &objectOffset);
PoseRT model2test_ground = model2test_fiducials * objectOffset;
// cout << "Ground truth: " << model2test_ground << endl;
CV_Assert(edgeModels.size() == 1);
float occlusionPercentage = computeOcclusionPercentage(kinectCamera, edgeModels[0], objectOffset, occlusionObjects, occlusionOffsets, model2test_fiducials);
CV_Assert(0.0 <= occlusionPercentage && occlusionPercentage <= 1.0);
cout << "occlusion percentage: " << occlusionPercentage << endl;
pcl::PointCloud<pcl::PointXYZ> testPointCloud;
#ifdef USE_3D_VISUALIZATION
dataImporter.importPointCloud(testImageIdx, testPointCloud);
#endif
#ifdef VISUALIZE_TEST_DATA
imshow("rgb", kinectBgrImage);
imshow("depth", kinectDepth * 20);
#endif
#ifdef VISUALIZE_POSE_REFINEMENT
#ifdef USE_3D_VISUALIZATION
{
vector<Point3f> cvTestPointCloud;
pcl2cv(testPointCloud, cvTestPointCloud);
cout << "test point cloud size: " << cvTestPointCloud.size() << endl;
publishPoints(cvTestPointCloud, viewer, Scalar(0, 255, 0), "test point cloud");
}
publishPoints(edgeModels[0].points, viewer, Scalar(0, 0, 255), "ground object", model2test_ground);
#endif
if(!kinectCameraFilename.empty())
{
// displayEdgels(glassMask, edgeModels[0].points, model2test_ground, kinectCamera, "kinect");
showEdgels(kinectBgrImage, edgeModels[0].points, model2test_ground, kinectCamera, "ground truth");
showEdgels(kinectBgrImage, edgeModels[0].stableEdgels, model2test_ground, kinectCamera, "ground truth surface");
}
namedWindow("ground truth");
#ifdef USE_3D_VISUALIZATION
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
viewer->resetStoppedFlag();
#endif
waitKey();
destroyWindow("ground truth");
#endif
vector<PoseRT> poses_cam;
vector<float> posesQualities;
vector<string> detectedObjectsNames;
TickMeter recognitionTime;
recognitionTime.start();
Detector::DebugInfo debugInfo;
try
{
detector.detect(kinectBgrImage, kinectDepth, registrationMask, testPointCloud, poses_cam, posesQualities, detectedObjectsNames, &debugInfo);
}
catch(const cv::Exception &)
{
}
recognitionTime.stop();
#ifdef VISUALIZE_POSE_REFINEMENT
Mat glassMask = debugInfo.glassMask;
imshow("glassMask", glassMask);
showSegmentation(kinectBgrImage, glassMask, "segmentation");
Mat detectionResults = kinectBgrImage.clone();
detector.visualize(poses_cam, detectedObjectsNames, detectionResults);
imshow("detection", detectionResults);
waitKey();
#endif
#ifndef PROFILE
if (edgeModels.size() == 1)
{
vector<Point2f> groundEdgels;
kinectCamera.projectPoints(edgeModels[0].points, model2test_ground, groundEdgels);
vector<float> chamferDistances;
for (size_t silhouetteIndex = 0; silhouetteIndex < debugInfo.initialSilhouettes.size(); ++silhouetteIndex)
{
vector<Point2f> silhouette = debugInfo.initialSilhouettes[silhouetteIndex];
double silhoutteDistance = 0.0;
for (int i = 0; i < silhouette.size(); ++i)
{
float minDist = std::numeric_limits<float>::max();
for (int j = 0; j < groundEdgels.size(); ++j)
{
float currentDist = norm(silhouette[i] - groundEdgels[j]);
if (currentDist < minDist)
{
minDist = currentDist;
}
}
silhoutteDistance += minDist;
}
silhoutteDistance /= silhouette.size();
chamferDistances.push_back(silhoutteDistance);
}
std::sort(chamferDistances.begin(), chamferDistances.end());
if (chamferDistances.empty())
{
chamferDistances.push_back(std::numeric_limits<float>::max());
}
cout << "Best geometric hashing pose (px): " << chamferDistances[0] << endl;
cout << "Number of initial poses: " << chamferDistances.size() << endl;
allChamferDistances.push_back(chamferDistances[0]);
geometricHashingPoseCount.push_back(chamferDistances.size());
}
if (poses_cam.size() == 0)
{
++segmentationFailuresCount;
continue;
}
if (!posesQualities.empty())
{
std::vector<float>::iterator bestDetection = std::min_element(posesQualities.begin(), posesQualities.end());
int bestDetectionIndex = std::distance(posesQualities.begin(), bestDetection);
// int detectedObjectIndex = detector.getTrainObjectIndex(detectedObjectsNames[bestDetectionIndex]);
// indicesOfRecognizedObjects.push_back(detectedObjectIndex);
cout << "Recognized object: " << detectedObjectsNames[bestDetectionIndex] << endl;
}
cout << "Recognition time: " << recognitionTime.getTimeSec() << "s" << endl;
allRecognitionTimes.push_back(recognitionTime.getTimeSec());
if (objectNames.size() == 1)
{
cout << "initial poses: " << debugInfo.initialPoses.size() << endl;
vector<PoseError> initialPoseErrors;
for (size_t i = 0 ; i < debugInfo.initialPoses.size(); ++i)
{
PoseError poseError;
evaluatePoseWithRotation(edgeModels[0], debugInfo.initialPoses[i], model2test_ground, poseError);
cout << poseError << endl;
initialPoseErrors.push_back(poseError);
// showEdgels(kinectBgrImage, edgeModels[0].points, debugInfo.initialPoses[i], kinectCamera, "gh pose");
// waitKey();
}
cout << "the end." << endl;
PoseError currentBestInitialError;
{
vector<PoseError>::iterator bestPoseIt = std::min_element(initialPoseErrors.begin(), initialPoseErrors.end());
int bestPoseIdx = std::distance(initialPoseErrors.begin(), bestPoseIt);
currentBestInitialError = initialPoseErrors[bestPoseIdx];
cout << "Best initial error: " << currentBestInitialError << endl;
bestInitialPoses.push_back(currentBestInitialError);
}
CV_Assert(poses_cam.size() == 1);
int objectIndex = 0;
initialPoseCount.push_back(poses_cam.size());
vector<PoseError> currentPoseErrors(poses_cam.size());
for (size_t i = 0 ; i < poses_cam.size(); ++i)
{
evaluatePoseWithRotation(edgeModels[objectIndex], poses_cam[i], model2test_ground, currentPoseErrors[i]);
cout << currentPoseErrors[i] << endl;
#ifdef VISUALIZE_POSE_REFINEMENT
namedWindow("pose is ready");
waitKey();
destroyWindow("pose is ready");
// displayEdgels(glassMask, edgeModels[objectIndex].points, initPoses_cam[objectIndex][i], kinectCamera, "initial");
#ifdef USE_3D_VISUALIZATION
publishPoints(edgeModels[objectIndex].points, viewer, Scalar(255, 0, 0), "final object", poses_cam[i]);
#endif
showEdgels(kinectBgrImage, edgeModels[objectIndex].points, poses_cam[i], kinectCamera, "final");
showEdgels(kinectBgrImage, edgeModels[objectIndex].stableEdgels, poses_cam[i], kinectCamera, "final surface");
namedWindow("initial pose");
#ifdef USE_3D_VISUALIZATION
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
#endif
waitKey();
destroyWindow("initial pose");
#endif
}
vector<PoseError>::iterator bestPoseIt = std::min_element(currentPoseErrors.begin(), currentPoseErrors.end());
int bestPoseIdx = std::distance(currentPoseErrors.begin(), bestPoseIt);
PoseError currentBestError = currentPoseErrors[bestPoseIdx];
cout << "Best pose: " << currentBestError << endl;
bestPoses.push_back(currentBestError);
cout << "Result: " << occlusionPercentage << ", " << debugInfo.initialPoses.size() << ", " <<
currentBestInitialError.getTranslationDifference() << ", " << currentBestInitialError.getRotationDifference(false) << ", " <<
currentBestError.getTranslationDifference() << ", " << currentBestError.getRotationDifference(false) << endl;
#ifdef WRITE_ERRORS
const float maxTrans = 0.02;
if (currentPoseErrors[bestPoseIdx].getTranslationDifference() > maxTrans)
{
Mat glassMask = debugInfo.glassMask;
std::stringstream str;
str << testImageIdx;
Mat segmentation = drawSegmentation(kinectBgrImage, glassMask);
imwrite(errorsVisualizationPath + "/" + objectNames[0] + "_" + str.str() + "_mask.png", segmentation);
Mat poseImage = kinectBgrImage.clone();
detector.visualize(poses_cam, detectedObjectsNames, poseImage);
imwrite(errorsVisualizationPath + "/" + objectNames[0] + "_" + str.str() + "_pose.png", poseImage);
const float depthNormalizationFactor = 100;
imwrite(errorsVisualizationPath + "/" + objectNames[0] + "_" + str.str() + "_depth.png", kinectDepth * depthNormalizationFactor);
}
#endif
}
#endif
cout << endl;
#ifdef PROFILE
return 0;
#endif
}
cout << "Segmentation failures: " << static_cast<float>(segmentationFailuresCount) / testIndices.size() << endl;
cout << "Bad segmentation rate: " << static_cast<float>(badSegmentationCount) / testIndices.size() << endl;
/*
cout << "Recognition statistics:" << endl;
for (size_t i = 0; i < objectNames.size(); ++i)
{
cout << countNonZero(Mat(indicesOfRecognizedObjects) == i) << " ";
}
cout << endl;
for (size_t i = 0; i < objectNames.size(); ++i)
{
cout << countNonZero(Mat(indicesOfRecognizedObjects) == i) / static_cast<float>(indicesOfRecognizedObjects.size()) << " ";
}
cout << endl;
*/
if (objectNames.size() == 1)
{
float meanInitialPoseCount = std::accumulate(initialPoseCount.begin(), initialPoseCount.end(), 0);
meanInitialPoseCount /= initialPoseCount.size();
cout << "mean initial pose count: " << meanInitialPoseCount << endl;
//TODO: move up
const double cmThreshold = 2.0;
// const double cmThreshold = 5.0;
PoseError::evaluateErrors(bestPoses, cmThreshold);
cout << "initial poses:" << endl;
//TODO: move up
PoseError::evaluateErrors(bestInitialPoses, 3.0 * cmThreshold);
}
cout << "Evaluation of geometric hashing" << endl;
std::sort(allChamferDistances.begin(), allChamferDistances.end());
const float successfulChamferDistance = 5.0f;
int ghSuccessCount = 0;
double meanChamferDistance = 0.0;
for (size_t i = 0; i < allChamferDistances.size(); ++i)
{
cout << i << "\t: " << allChamferDistances[i] << endl;
if (allChamferDistances[i] < successfulChamferDistance)
{
++ghSuccessCount;
meanChamferDistance += allChamferDistances[i];
}
}
if (ghSuccessCount != 0)
{
meanChamferDistance /= ghSuccessCount;
}
int posesSum = std::accumulate(geometricHashingPoseCount.begin(), geometricHashingPoseCount.end(), 0);
float meanInitialPoseCount = static_cast<float>(posesSum) / initialPoseCount.size();
cout << "Mean number of initial poses: " << meanInitialPoseCount << endl;
float ghSuccessRate = static_cast<float>(ghSuccessCount) / allChamferDistances.size();
cout << "Success rate: " << ghSuccessRate << endl;
cout << "Mean chamfer distance (px): " << meanChamferDistance << endl;
double timesSum = std::accumulate(allRecognitionTimes.begin(), allRecognitionTimes.end(), 0.0);
double meanRecognitionTime = timesSum / allRecognitionTimes.size();
cout << "Mean recognition time (s): " << meanRecognitionTime << endl;
std::system("date");
return 0;
}
//TODO: undistort and scale the image
void Silhouette::affine2poseRT(const EdgeModel &edgeModel, const PinholeCamera &camera, const cv::Mat &affineTransformation, bool useClosedFormPnP, PoseRT &pose_cam) const
{
PoseRT poseWithExtrinsics_cam;
if (useClosedFormPnP)
{
CV_Assert(camera.cameraMatrix.type() == CV_64FC1);
const float eps = 1e-6;
// CV_Assert(fabs(camera.cameraMatrix.at<double>(0, 0) - camera.cameraMatrix.at<double>(1, 1)) < eps);
CV_Assert(norm(camera.distCoeffs) < eps);
Mat homography = affine2homography(affineTransformation);
if (homography.type() != CV_64FC1)
{
Mat homographyDouble;
homography.convertTo(homographyDouble, CV_64FC1);
homography = homographyDouble;
}
//TODO: which inversion method is better?
Mat fullTransform = camera.cameraMatrix.inv() * homography * camera.cameraMatrix;
const int dim = 3;
CV_Assert(fullTransform.rows == dim && fullTransform.cols == dim);
CV_Assert(fullTransform.type() == CV_64FC1);
Mat rotationalComponentWithScale = fullTransform(Range(0, 2), Range(0, 2));
double det = determinant(rotationalComponentWithScale);
CV_Assert(det > eps);
double scale = 1.0 / sqrt(det);
Point3d objectCenter = edgeModel.getObjectCenter();
Point3d initialObjectCenter;
transformPoint(initialPose_cam.getProjectiveMatrix(), objectCenter, initialObjectCenter);
double meanZ = initialObjectCenter.z;
CV_Assert(meanZ > eps);
Point3d tvec;
tvec.z = (scale - 1.0) * meanZ;
tvec.x = fullTransform.at<double>(0, 2) * (scale * meanZ);
tvec.y = fullTransform.at<double>(1, 2) * (scale * meanZ);
Mat R = Mat::eye(dim, dim, fullTransform.type());
Mat rotation2d = R(Range(0, 2), Range(0, 2));
Mat pureRotationalComponent = rotationalComponentWithScale * scale;
pureRotationalComponent.copyTo(rotation2d);
Mat tvecMat;
point2col(tvec, tvecMat);
PoseRT pose2d_cam(R, tvecMat);
poseWithExtrinsics_cam = pose2d_cam * initialPose_cam;
pose_cam = camera.extrinsics.inv() * pose2d_cam * initialPose_cam;
}
else
{
vector<Point2f> projectedObjectPoints;
camera.projectPoints(edgeModel.points, initialPose_cam, projectedObjectPoints);
Mat transformedObjectPoints;
transform(Mat(projectedObjectPoints), transformedObjectPoints, affineTransformation);
solvePnP(Mat(edgeModel.points), transformedObjectPoints, camera.cameraMatrix, camera.distCoeffs, poseWithExtrinsics_cam.rvec, poseWithExtrinsics_cam.tvec, useClosedFormPnP);
pose_cam = camera.extrinsics.inv() * poseWithExtrinsics_cam;
}
}
