FaceRecognition.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package VisionX::ArmarXObjects::FaceRecognition
17  * @author Markus Grotz ( markus dot grotz at kit dot edu )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "FaceRecognition.h"
24 #include <Image/PrimitivesDrawerCV.h>
25 #include <Image/PrimitivesDrawer.h>
27 
30 
32 
34 
35 #include <SimoxUtility/algorithm/string/string_tools.h>
36 
37 #include <Image/ImageProcessor.h>
38 #include <Image/IplImageAdaptor.h>
39 
40 #include <Eigen/Geometry>
41 
42 using namespace armarx;
43 
44 
46 {
47 
48  stereoMatcher = new CStereoMatcher();
49 
50  model = cv::face::FisherFaceRecognizer(0, 1000.0);
51 
52  armarx::CMakePackageFinder finder("VisionX");
54 
55  std::string trainingDataPath = getProperty<std::string>("trainingDataPath").getValue();
56 
57  if (!ArmarXDataPath::getAbsolutePath(trainingDataPath, trainingDataPath))
58  {
59  ARMARX_ERROR << "Could not find data file in ArmarXDataPath: " << trainingDataPath;
60  }
61 
62  std::string classifierFileName = getProperty<std::string>("classifierFileName").getValue();
63 
64  if (!ArmarXDataPath::getAbsolutePath(classifierFileName, classifierFileName))
65  {
66  ARMARX_ERROR << "Could not find data file in ArmarXDataPath: " << classifierFileName;
67  }
68 
69  if (!std::filesystem::is_directory(trainingDataPath))
70  {
71  ARMARX_FATAL << "unable to load training model";
72 
73  }
74 
75  std::vector<std::string> fileNames;
76 
77  for (auto iter = std::filesystem::directory_iterator(trainingDataPath);
78  iter != std::filesystem::directory_iterator(); ++iter)
79  {
80  fileNames.push_back(iter->path().string());
81  }
82 
83  std::sort(fileNames.begin(), fileNames.end());
84 
85  std::string currentLabel;
86 
87  std::vector<cv::Mat> images;
88  std::vector<int> index;
89 
90  for (std::string& fileName : fileNames)
91  {
92  const auto stem = std::filesystem::path(fileName).stem().string();
93  std::vector<std::string> strs = simox::alg::split(stem, "_");
94  std::string label = strs[0];
95 
96  if (currentLabel != label)
97  {
98  labels[labels.size()] = label;
99  currentLabel = label;
100  }
101  images.push_back(cv::imread(fileName, CV_LOAD_IMAGE_GRAYSCALE));
102  index.push_back(labels.size() - 1);
103  }
104 
105  ARMARX_LOG << "total number of classes: " << labels.size();
106 
107  model->train(images, index);
108 
109  faceImageSize = images[0].size();
110 
111  classifier.load(classifierFileName);
112 
113 
114 }
115 
117 {
118  stereoMatcher->InitCameraParameters(getStereoCalibration(), false);
119 }
120 
122 {
123 
124  delete stereoMatcher;
125 }
126 
128 {
129  return true;
130 }
131 
133 {
134  ///@TODO
135  throw std::logic_error {"bool armarx::FaceRecognition::addObjectClass not implemented yet"};
136 }
137 
138 
139 memoryx::ObjectLocalizationResultList armarx::FaceRecognition::localizeObjectClasses(const std::vector<std::string>& objectClassNames, CByteImage** cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage** resultImages)
140 {
141 
142  memoryx::ObjectLocalizationResultList resultList;
143  cv::Mat result = cv::cvarrToMat(IplImageAdaptor::Adapt(resultImages[0]));
144 
145 
146  const cv::Mat tempRGBImage = cv::cvarrToMat(IplImageAdaptor::Adapt(cameraImages[0]));
147  cv::Mat original;
148  cv::cvtColor(tempRGBImage, original, CV_RGB2BGR);
149 
150  cv::Mat gray;
151  cv::cvtColor(original, gray, CV_BGR2GRAY);
152 
153  std::vector<cv::Rect_<int>> faces;
154  classifier.detectMultiScale(gray, faces, 1.05, 2);
155 
156  const std::string refFrame = getProperty<std::string>("ReferenceFrameName").getValue();
157  const std::string agentName = getProperty<std::string>("AgentName").getValue();
158 
159  ARMARX_INFO << deactivateSpam(5, std::to_string(faces.size())) << "found " << faces.size() << " possible faces";
160  for (cv::Rect rect : faces)
161  {
162  cv::Mat face = gray(rect);
163 
164  cv::Mat faceResized;
165  cv::resize(face, faceResized, faceImageSize, 1.0, 1.0, cv::INTER_CUBIC);
166 
167  int predictedLabel = -1;
168  double predictedConfidence = 0.0;
169  model->predict(faceResized, predictedLabel, predictedConfidence);
170 
171  std::string temp = labels[predictedLabel] + "=%0.1f";
172  std::string label = cv::format(temp.c_str(), predictedConfidence);
173 
174 
175  if (predictedLabel < 0)
176  {
177  cv::rectangle(result, rect, CV_RGB(0, 0, 255), 1);
178  }
179  else
180  {
181  cv::rectangle(result, rect, CV_RGB(0, 255, 0), 1);
182 
183  int posX = std::max(rect.tl().x - 10, 0);
184  int posY = std::max(rect.tl().y - 10, 0);
185  putText(result, label, cv::Point(posX, posY), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(0, 255, 0), 2.0);
186 
187  if (std::find(objectClassNames.begin(), objectClassNames.end(), label) != objectClassNames.end())
188  {
189  // todo see ObjectLearningByPushing/FeatureCalculation.cpp
190  CByteImage* imgRightGray = new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eGrayScale);
191  CByteImage* imgLeftGray = new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eGrayScale);
192  ::ImageProcessor::ConvertImage(cameraImages[0], imgLeftGray);
193  ::ImageProcessor::ConvertImage(cameraImages[1], imgRightGray);
194 
195  Vec2d vCorrespondingPointRight;
196  Vec3d vPoint3D;
197  const int nDispMin = stereoMatcher->GetDisparityEstimate(10000);
198  const int nDispMax = stereoMatcher->GetDisparityEstimate(500);
199 
200  // img l, img r, px, py, size of correlation window, min disparity, max disparity,
201  // corresponding point 2d, 3d point, correlation threshold, images are undistorted
202  int nMatchingResult = stereoMatcher->Match(imgLeftGray, imgRightGray, (int) rect.x + rect.width / 2.0, (int) rect.y + rect.height / 2.0,
203  std::max(rect.width, rect.height), nDispMin, nDispMax, vCorrespondingPointRight, vPoint3D, 0.7f, true);
204 
205 
206  delete imgRightGray;
207  delete imgLeftGray;
208 
209  if (nMatchingResult >= 0)
210  {
211  Eigen::Vector3f position(vPoint3D.x, vPoint3D.y, vPoint3D.z);
212  Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity() * Eigen::AngleAxisf(-M_PI, Eigen::Vector3f::UnitZ());
213 
214  memoryx::ObjectLocalizationResult result;
215 
216  result.position = new armarx::FramedPosition(position, refFrame, agentName);
217  result.orientation = new armarx::FramedOrientation(orientation, refFrame, agentName);
218  result.recognitionCertainty = 1.0 / predictedConfidence;
219  result.positionNoise = calculateLocalizationUncertainty(position);
220  result.objectClassName = label;
221 
222  resultList.push_back(result);
223  }
224  }
225  }
226  }
227 
228  return resultList;
229 }
230 
231 
232 
234 {
237 }
238 
armarx::FaceRecognition::onExitObjectLocalizerProcessor
void onExitObjectLocalizerProcessor() override
Definition: FaceRecognition.cpp:121
armarx::FaceRecognition::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: FaceRecognition.cpp:233
armarx::FaceRecognition::addObjectClass
bool addObjectClass(const memoryx::EntityPtr &objectClassEntity, const memoryx::GridFileManagerPtr &fileManager) override
Add a memory entity representing the hand marker in order to set its properties.
Definition: FaceRecognition.cpp:132
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:53
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:694
armarx::FaceRecognition::localizeObjectClasses
memoryx::ObjectLocalizationResultList localizeObjectClasses(const std::vector< std::string > &objectClassNames, CByteImage **cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage **resultImages) override
Add a memory entity representing the hand marker in order to set its properties.
Definition: FaceRecognition.cpp:139
ARMARX_FATAL
#define ARMARX_FATAL
Definition: Logging.h:192
IceInternal::Handle
Definition: forward_declarations.h:8
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::FaceRecognition::onConnectObjectLocalizerProcessor
void onConnectObjectLocalizerProcessor() override
Definition: FaceRecognition.cpp:116
armarx::FaceRecognitionPropertyDefinitions
Definition: FaceRecognition.h:43
FaceRecognition.h
M_PI
#define M_PI
Definition: MathTools.h:17
FramedPose.h
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:695
armarx::FaceRecognition::onInitObjectLocalizerProcessor
void onInitObjectLocalizerProcessor() override
Definition: FaceRecognition.cpp:45
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:176
MemoryXCoreObjectFactories.h
visionx::Point
Eigen::Vector3f Point
Definition: ObjectShapeClassification.h:69
max
T max(T t1, T t2)
Definition: gdiam.h:48
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:163
armarx::FaceRecognition::initRecognizer
bool initRecognizer() override
Initializes segmentable recognition.
Definition: FaceRecognition.cpp:127
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
CMakePackageFinder.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
armarx::ArmarXDataPath::addDataPaths
static void addDataPaths(const std::string &dataPathList)
Definition: ArmarXDataPath.cpp:559
IceUtil::Handle< class PropertyDefinitionContainer >
TypeMapping.h
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36