public NaturalPipeline(IComponentManager xpcfComponentManager) : base(xpcfComponentManager)
{
imageViewerKeypoints = Create <IImageViewer>("SolARImageViewerOpencv", "keypoints");
imageViewerResult = Create <IImageViewer>("SolARImageViewerOpencv");
marker = Create <IMarker2DNaturalImage>("SolARMarker2DNaturalImageOpencv");
kpDetector = Create <IKeypointDetector>("SolARKeypointDetectorOpencv");
kpDetectorRegion = Create <IKeypointDetectorRegion>("SolARKeypointDetectorRegionOpencv");
descriptorExtractor = Create <IDescriptorsExtractor>("SolARDescriptorsExtractorAKAZE2Opencv");
matcher = Create <IDescriptorMatcher>("SolARDescriptorMatcherKNNOpencv");
geomMatchesFilter = Create <IMatchesFilter>("SolARGeometricMatchesFilterOpencv");
poseEstimationPlanar = Create <I3DTransformSACFinderFrom2D3D>("SolARPoseEstimationPlanarPointsOpencv");
opticalFlow = Create <IOpticalFlowEstimator>("SolAROpticalFlowPyrLKOpencv");
projection = Create <IProject>("SolARProjectOpencv");
unprojection = Create <IUnproject>("SolARUnprojectPlanarPointsOpencv");
img_mapper = Create <IImage2WorldMapper>("SolARImage2WorldMapper4Marker2D");
basicMatchesFilter = Create <IMatchesFilter>("SolARBasicMatchesFilter");
keypointsReindexer = Create <IKeypointsReIndexer>("SolARKeypointsReIndexer");
overlay3DComponent = Create <I3DOverlay>("SolAR3DOverlayOpencv");
/* in dynamic mode, we need to check that components are well created*/
/* this is needed in dynamic mode */
if (new object[] { imageViewerKeypoints, imageViewerResult, marker, kpDetector, kpDetectorRegion, descriptorExtractor, matcher,
geomMatchesFilter, poseEstimationPlanar, opticalFlow, projection, unprojection, img_mapper,
basicMatchesFilter, keypointsReindexer, overlay3DComponent }.Contains(null))
{
LOG_ERROR("One or more component creations have failed");
return;
}
LOG_INFO("All components have been created");
// Declare data structures used to exchange information between components
refImage = SharedPtr.Alloc <Image>().AddTo(subscriptions);
previousCamImage = SharedPtr.Alloc <Image>().AddTo(subscriptions);
//kpImageCam = SharedPtr.Alloc<Image>().AddTo(subscriptions);
refDescriptors = SharedPtr.Alloc <DescriptorBuffer>().AddTo(subscriptions);
camDescriptors = SharedPtr.Alloc <DescriptorBuffer>().AddTo(subscriptions);
matches = new DescriptorMatchVector().AddTo(subscriptions);
// where to store detected keypoints in ref image and camera image
refKeypoints = new KeypointArray().AddTo(subscriptions);
camKeypoints = new KeypointArray().AddTo(subscriptions);
markerWorldCorners = new Point3DfArray().AddTo(subscriptions);
// load marker
marker.loadMarker().Check();
marker.getWorldCorners(markerWorldCorners).Check();
marker.getImage(refImage).Check();
// detect keypoints in reference image
kpDetector.detect(refImage, refKeypoints);
// extract descriptors in reference image
descriptorExtractor.extract(refImage, refKeypoints, refDescriptors);
// initialize image mapper with the reference image size and marker size
var img_mapper_config = img_mapper.BindTo <IConfigurable>().AddTo(subscriptions);
var refSize = refImage.getSize();
var mkSize = marker.getSize();
img_mapper_config.getProperty("digitalWidth").setIntegerValue((int)refSize.width);
img_mapper_config.getProperty("digitalHeight").setIntegerValue((int)refSize.height);
img_mapper_config.getProperty("worldWidth").setFloatingValue(mkSize.width);
img_mapper_config.getProperty("worldHeight").setFloatingValue(mkSize.height);
// vector of 4 corners in the marker
refImgCorners = new Point2DfArray();
float w = refImage.getWidth(), h = refImage.getHeight();
Point2Df corner0 = new Point2Df(0, 0);
Point2Df corner1 = new Point2Df(w, 0);
Point2Df corner2 = new Point2Df(w, h);
Point2Df corner3 = new Point2Df(0, h);
refImgCorners.Add(corner0);
refImgCorners.Add(corner1);
refImgCorners.Add(corner2);
refImgCorners.Add(corner3);
}
public override FrameworkReturnCode Proceed(Image camImage, Transform3Df pose, ICamera camera)
{
// initialize overlay 3D component with the camera intrinsec parameters (please refeer to the use of intrinsec parameters file)
overlay3DComponent.setCameraParameters(camera.getIntrinsicsParameters(), camera.getDistorsionParameters());
// initialize pose estimation based on planar points with the camera intrinsec parameters (please refeer to the use of intrinsec parameters file)
poseEstimationPlanar.setCameraParameters(camera.getIntrinsicsParameters(), camera.getDistorsionParameters());
// initialize projection component with the camera intrinsec parameters (please refeer to the use of intrinsec parameters file)
projection.setCameraParameters(camera.getIntrinsicsParameters(), camera.getDistorsionParameters());
// initialize unprojection component with the camera intrinsec parameters (please refeer to the use of intrinsec parameters file)
unprojection.setCameraParameters(camera.getIntrinsicsParameters(), camera.getDistorsionParameters());
if (!isTrack)
{
kpDetector.detect(camImage, camKeypoints);
descriptorExtractor.extract(camImage, camKeypoints, camDescriptors);
matcher.match(refDescriptors, camDescriptors, matches);
basicMatchesFilter.filter(matches, matches, refKeypoints, camKeypoints);
geomMatchesFilter.filter(matches, matches, refKeypoints, camKeypoints);
var ref2Dpoints = new Point2DfArray();
var cam2Dpoints = new Point2DfArray();
var ref3Dpoints = new Point3DfArray();
if (matches.Count > 10)
{
keypointsReindexer.reindex(refKeypoints, camKeypoints, matches, ref2Dpoints, cam2Dpoints).Check();
img_mapper.map(ref2Dpoints, ref3Dpoints).Check();
if (poseEstimationPlanar.estimate(cam2Dpoints, ref3Dpoints, imagePoints_inliers, worldPoints_inliers, pose) != FrameworkReturnCode._SUCCESS)
{
valid_pose = false;
//LOG_DEBUG("Wrong homography for this frame");
}
else
{
isTrack = true;
needNewTrackedPoints = true;
valid_pose = true;
previousCamImage = camImage.copy();
//LOG_INFO("Start tracking", pose.matrix());
}
}
}
else
{
// initialize points to track
if (needNewTrackedPoints)
{
imagePoints_track.Clear();
worldPoints_track.Clear();
KeypointArray newKeypoints = new KeypointArray();
// Get the projection of the corner of the marker in the current image
projection.project(markerWorldCorners, projectedMarkerCorners, pose);
// Detect the keypoints within the contours of the marker defined by the projected corners
kpDetectorRegion.detect(previousCamImage, projectedMarkerCorners, newKeypoints);
if (newKeypoints.Count > updateTrackedPointThreshold)
{
foreach (var keypoint in newKeypoints)
{
//imagePoints_track.push_back(xpcf::utils::make_shared<Point2Df>(keypoint->getX(), keypoint->getY()));
imagePoints_track.Add(new Point2Df(keypoint.getX(), keypoint.getY()));
}
// get back the 3D positions of the detected keypoints in world space
unprojection.unproject(imagePoints_track, worldPoints_track, pose);
//LOG_DEBUG("Reinitialize points to track");
}
else
{
isTrack = false;
//LOG_DEBUG("Cannot reinitialize points to track");
}
needNewTrackedPoints = false;
}
// Tracking mode
if (isTrack)
{
Point2DfArray trackedPoints = new Point2DfArray();
Point2DfArray pts2D = new Point2DfArray();
Point3DfArray pts3D = new Point3DfArray();
UCharList status = new UCharList();
FloatList err = new FloatList();
// tracking 2D-2D
opticalFlow.estimate(previousCamImage, camImage, imagePoints_track, trackedPoints, status, err);
for (int i = 0; i < status.Count; i++)
{
if (status[i] == 1)
{
pts2D.Add(trackedPoints[i]);
pts3D.Add(worldPoints_track[i]);
}
}
// calculate camera pose
// Estimate the pose from the 2D-3D planar correspondence
if (poseEstimationPlanar.estimate(pts2D, pts3D, imagePoints_track, worldPoints_track, pose) != FrameworkReturnCode._SUCCESS)
{
isTrack = false;
valid_pose = false;
needNewTrackedPoints = false;
//LOG_INFO("Tracking lost");
}
else
{
valid_pose = true;
previousCamImage = camImage.copy();
if (worldPoints_track.Count < updateTrackedPointThreshold)
{
needNewTrackedPoints = true;
}
}
}
//else
//LOG_INFO("Tracking lost");
}
if (valid_pose)
{
// We draw a box on the place of the recognized natural marker
overlay3DComponent.draw(pose, camImage);
}
//if (imageViewerResult.display(camImage) == FrameworkReturnCode._STOP) return FrameworkReturnCode._STOP;
return(FrameworkReturnCode._SUCCESS);
}