static Pose CallPluginAtEndOfFrames(Transform3Df pose)
{
Matrix4x4 cameraPoseFromSolAR = Matrix4x4.identity;
var pos = pose.translation();
var rot = pose.rotation();
//cameraPoseFromSolAR.SetRow(0, new Vector4(rot.coeff(0, 0), rot.coeff(0, 1), rot.coeff(0, 2), pos.coeff(0, 0)));
//cameraPoseFromSolAR.SetRow(1, new Vector4(rot.coeff(1, 0), rot.coeff(1, 1), rot.coeff(1, 2), pos.coeff(0, 1)));
//cameraPoseFromSolAR.SetRow(2, new Vector4(rot.coeff(2, 0), rot.coeff(2, 1), rot.coeff(2, 2), pos.coeff(0, 2)));
//cameraPoseFromSolAR.SetRow(3, new Vector4(0, 0, 0, 1));
for (int r = 0; r < 3; ++r)
{
for (int c = 0; c < 3; ++c)
{
cameraPoseFromSolAR[r, c] = rot.coeff(r, c);
}
cameraPoseFromSolAR[r, 3] = pos.coeff(0, r);
}
Matrix4x4 unityCameraPose = invertMatrix * cameraPoseFromSolAR;
Vector3 forward = new Vector3(unityCameraPose.m02, unityCameraPose.m12, unityCameraPose.m22);
Vector3 up = new Vector3(unityCameraPose.m01, unityCameraPose.m11, unityCameraPose.m21);
var q = Quaternion.LookRotation(forward, -up);
var p = new Vector3(unityCameraPose.m03, unityCameraPose.m13, unityCameraPose.m23);
return(new Pose(p, q));
}
public virtual void set(Transform3Df pose, Image image)
{
solar_api_sinkPINVOKE.ISinkPoseImage_set__SWIG_0(swigCPtr, Transform3Df.getCPtr(pose), Image.getCPtr(image));
if (solar_api_sinkPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_sinkPINVOKE.SWIGPendingException.Retrieve();
}
}
public virtual void draw(Transform3Df pose, Image displayImage)
{
solar_api_displayPINVOKE.I3DOverlay_draw(swigCPtr, Transform3Df.getCPtr(pose), Image.getCPtr(displayImage));
if (solar_api_displayPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_displayPINVOKE.SWIGPendingException.Retrieve();
}
}
public virtual void filter(Transform3Df pose1, Transform3Df pose2, CloudPointVector input, CloudPointVector output, IntVector index)
{
solar_api_solver_mapPINVOKE.IMapFilter_filter__SWIG_1(swigCPtr, Transform3Df.getCPtr(pose1), Transform3Df.getCPtr(pose2), CloudPointVector.getCPtr(input), CloudPointVector.getCPtr(output), IntVector.getCPtr(index));
if (solar_api_solver_mapPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_solver_mapPINVOKE.SWIGPendingException.Retrieve();
}
}
public virtual SinkReturnCode tryGet(Transform3Df pose)
{
SinkReturnCode ret = (SinkReturnCode)solar_api_sinkPINVOKE.ISinkPoseImage_tryGet(swigCPtr, Transform3Df.getCPtr(pose));
if (solar_api_sinkPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_sinkPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public PIPELINEMANAGER_RETURNCODE udpate(Transform3Df pose)
{
PIPELINEMANAGER_RETURNCODE ret = (PIPELINEMANAGER_RETURNCODE)SolARPipelineManagerPINVOKE.SolARPluginPipelineManager_udpate(swigCPtr, Transform3Df.getCPtr(pose));
if (SolARPipelineManagerPINVOKE.SWIGPendingException.Pending)
{
throw SolARPipelineManagerPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual SinkReturnCode udpate(Transform3Df pose)
{
SinkReturnCode ret = (SinkReturnCode)solar_api_sinkPINVOKE.ISinkPoseTextureBuffer_udpate(swigCPtr, Transform3Df.getCPtr(pose));
if (solar_api_sinkPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_sinkPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual FrameworkReturnCode process(Transform3Df outputData)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_fusionPINVOKE.IVisualInertialFusion_process(swigCPtr, Transform3Df.getCPtr(outputData));
if (solar_api_fusionPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_fusionPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual FrameworkReturnCode transform(Point3DfArray inputPoints, Transform3Df transformation, Point3DfArray outputPoints)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_geomPINVOKE.I3DTransform_transform(swigCPtr, Point3DfArray.getCPtr(inputPoints), Transform3Df.getCPtr(transformation), Point3DfArray.getCPtr(outputPoints));
if (solar_api_geomPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_geomPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual FrameworkReturnCode display(CloudPointVector points, Transform3Df pose)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_displayPINVOKE.I3DPointsViewer_display__SWIG_4(swigCPtr, CloudPointVector.getCPtr(points), Transform3Df.getCPtr(pose));
if (solar_api_displayPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_displayPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual SinkReturnCode update(Transform3Df pose)
{
SinkReturnCode ret = (SinkReturnCode)solar_api_pipelinePINVOKE.IPipeline_update(swigCPtr, Transform3Df.getCPtr(pose));
if (solar_api_pipelinePINVOKE.SWIGPendingException.Pending)
{
throw solar_api_pipelinePINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual FrameworkReturnCode relocalize(Frame frame, Transform3Df pose)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_relocPINVOKE.IRelocalizer_relocalize(swigCPtr, Frame.getCPtr(frame), Transform3Df.getCPtr(pose));
if (solar_api_relocPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_relocPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static Pose ToUnity(this Transform3Df pose)
{
var inv = new Matrix4x4();
inv.SetRow(0, new Vector4(+1, +0, +0, +0));
inv.SetRow(1, new Vector4(+0, -1, +0, +0));
inv.SetRow(2, new Vector4(+0, +0, +1, +0));
inv.SetRow(3, new Vector4(+0, +0, +0, +1));
var rot = pose.rotation();
var trans = pose.translation();
var m = new Matrix4x4();
for (int r = 0; r < 3; ++r)
{
for (int c = 0; c < 3; ++c)
{
m[r, c] = rot.coeff(r, c);
}
m[r, 3] = trans.coeff(r, 0);
}
m.SetRow(3, new Vector4(0, 0, 0, 1));
m = m.inverse;
m = inv * m;
//m = m.inverse;
var v = m.GetColumn(3);
var q = Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));
q = Quaternion.Inverse(q);
v = q * -v;
return(new Pose(v, q));
}
public virtual double triangulate(KeypointArray keypointsView1, KeypointArray keypointsView2, DescriptorBuffer descriptor1, DescriptorBuffer descriptor2, DescriptorMatchVector matches, PairUIntUInt working_views, Transform3Df poseView1, Transform3Df poseView2, CloudPointVector pcloud)
{
double ret = solar_api_solver_mapPINVOKE.ITriangulator_triangulate__SWIG_2(swigCPtr, KeypointArray.getCPtr(keypointsView1), KeypointArray.getCPtr(keypointsView2), DescriptorBuffer.getCPtr(descriptor1), DescriptorBuffer.getCPtr(descriptor2), DescriptorMatchVector.getCPtr(matches), PairUIntUInt.getCPtr(working_views), Transform3Df.getCPtr(poseView1), Transform3Df.getCPtr(poseView2), CloudPointVector.getCPtr(pcloud));
if (solar_api_solver_mapPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_solver_mapPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
void Update()
{
if (isScanning && UpdateReady)
{
if (m_pipelineManager != null)
{
if (m_Unity_Webcam)
{
GetPhysicalCameraFrame();
}
Transform3Df pose = new Transform3Df();
var _returnCode = m_pipelineManager.udpate(pose);
if (_returnCode == PIPELINEMANAGER_RETURNCODE._NEW_POSE || _returnCode == PIPELINEMANAGER_RETURNCODE._NEW_POSE_AND_IMAGE)
{
if (!isMarkerFound)
{
isMarkerFound = true;
findDeviceCanvas.SetActive(false);
cardboardLoader.SetActive(true);
deviceCameraScript.gameObject.SetActive(false);
phoneManagerScript.ChangeVisibility(true);
}
Matrix4x4 cameraPoseFromSolAR = new Matrix4x4();
cameraPoseFromSolAR.SetRow(0, new Vector4(pose.rotation().coeff(0, 0), pose.rotation().coeff(0, 1), pose.rotation().coeff(0, 2), pose.translation().coeff(0, 0)));
cameraPoseFromSolAR.SetRow(1, new Vector4(pose.rotation().coeff(1, 0), pose.rotation().coeff(1, 1), pose.rotation().coeff(1, 2), pose.translation().coeff(1, 0)));
cameraPoseFromSolAR.SetRow(2, new Vector4(pose.rotation().coeff(2, 0), pose.rotation().coeff(2, 1), pose.rotation().coeff(2, 2), pose.translation().coeff(2, 0)));
cameraPoseFromSolAR.SetRow(3, new Vector4(0, 0, 0, 1));
Matrix4x4 invertMatrix = new Matrix4x4();
invertMatrix.SetRow(0, new Vector4(1, 0, 0, 0));
invertMatrix.SetRow(1, new Vector4(0, -1, 0, 0));
invertMatrix.SetRow(2, new Vector4(0, 0, 1, 0));
invertMatrix.SetRow(3, new Vector4(0, 0, 0, 1));
Matrix4x4 unityCameraPose = invertMatrix * cameraPoseFromSolAR;
Vector3 forward = new Vector3(unityCameraPose.m02, unityCameraPose.m12, unityCameraPose.m22);
Vector3 up = new Vector3(unityCameraPose.m01, unityCameraPose.m11, unityCameraPose.m21);
var rotation = Quaternion.LookRotation(forward, -up);
var position = -new Vector3(unityCameraPose.m03, unityCameraPose.m13, unityCameraPose.m23);
m_PrevSolARObj.localPosition = position;
m_RotationControl.localRotation = rotation;
var prevParent = m_PrevSolARObj.parent;
m_PrevSolARObj.parent = m_RotationControl;
m_RotationControl.localRotation = Quaternion.identity;
m_PrevSolARObj.parent = prevParent;
m_PrevSolARObj.localRotation = Quaternion.Inverse(rotation);
m_PrevSolARObj.Rotate(new Vector3(-90, 0, 0), Space.Self);
if (Screen.orientation == ScreenOrientation.LandscapeRight)
{
m_PrevSolARObj.localPosition -= new Vector3(cameraXShift, 0, 0);
}
else
{
m_PrevSolARObj.localPosition += new Vector3(cameraXShift, 0, 0);
}
var rotDist = Quaternion.Angle(prevRot, m_PrevSolARObj.rotation);
if (rotDist >= 10)
{
prevRot = m_PrevSolARObj.rotation;
socketSignalerScript.ResetGyroInverse();
}
var posDist = Vector3.Distance(m_SolARObj.position, m_PrevSolARObj.position);
float distLerpStatus = (Time.time - distLerpTime) * lerpSpeed;
////var angleDifferences = GetAngleDifferences(m_SolARObj.rotation, m_PrevSolARObj.rotation);
if (posDist >= 0.008 && distLerpStatus >= 1)
{
//Debug.Log("Pos dist: " + posDist);
//m_SolARObj.position = m_PrevSolARObj.position;
distLerpTime = Time.time;
curPos = m_SolARObj.position;
prevPos = m_PrevSolARObj.position;
}
distLerpStatus = (Time.time - distLerpTime) * lerpSpeed;
m_SolARObj.position = Vector3.Lerp(curPos, prevPos, distLerpStatus);
}
}
}
SetFusedRotation();
}
public virtual void filter(DescriptorMatchVector inputMatches, DescriptorMatchVector outputMatches, KeypointArray inputKeyPoints1, KeypointArray inputKeyPoints2, Transform3Df pose1, Transform3Df pose2, Matrix3x3f intrinsicParams)
{
solar_api_featuresPINVOKE.IMatchesFilter_filter__SWIG_1(swigCPtr, DescriptorMatchVector.getCPtr(inputMatches), DescriptorMatchVector.getCPtr(outputMatches), KeypointArray.getCPtr(inputKeyPoints1), KeypointArray.getCPtr(inputKeyPoints2), Transform3Df.getCPtr(pose1), Transform3Df.getCPtr(pose2), Matrix3x3f.getCPtr(intrinsicParams));
if (solar_api_featuresPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_featuresPINVOKE.SWIGPendingException.Retrieve();
}
}
public override FrameworkReturnCode Proceed(Image camImage, Transform3Df pose, ICamera camera)
{
// Action<IntVector, double> localBundleAdjuster = (IntVector framesIdxToBundle, double reprojError) =>
// {
// Transform3DfList correctedPoses = new Transform3DfList();
// CloudPointVector correctedCloud = new CloudPointVector();
// CameraParameters correctedCamera = new CameraParameters();
// reprojError = bundler.solve(mapper.getKeyframes(),
// mapper.getGlobalMap().getPointCloud(),
// camera.getIntrinsicsParameters(),
// camera.getDistorsionParameters(),
// framesIdxToBundle,
// correctedPoses,
// correctedCloud,
// correctedCamera.intrinsic,
// correctedCamera.distorsion);
// mapper.update(correctedPoses, correctedCloud);
// };
// binaryMarker.loadMarker().Check();
// patternDescriptorExtractor.extract(binaryMarker.getPattern(), markerPatternDescriptor);
// var patternSize = binaryMarker.getPattern().getSize();
// var binaryMarkerSize = binaryMarker.getSize();
// overlay3D.BindTo<IConfigurable>().getProperty("size").setFloatingValue(binaryMarkerSize.width, 0);
// overlay3D.BindTo<IConfigurable>().getProperty("size").setFloatingValue(binaryMarkerSize.height, 1);
// overlay3D.BindTo<IConfigurable>().getProperty("size").setFloatingValue(binaryMarkerSize.height / 2.0f, 2);
// patternDescriptorExtractor.BindTo<IConfigurable>().getProperty("patternSize").setIntegerValue(patternSize);
// patternReIndexer.BindTo<IConfigurable>().getProperty("sbPatternSize").setIntegerValue(patternSize);
// img2worldMapper.BindTo<IConfigurable>().getProperty("digitalWidth").setIntegerValue(patternSize);
// img2worldMapper.BindTo<IConfigurable>().getProperty("digitalHeight").setIntegerValue(patternSize);
// img2worldMapper.BindTo<IConfigurable>().getProperty("worldWidth").setFloatingValue(binaryMarkerSize.width);
// img2worldMapper.BindTo<IConfigurable>().getProperty("worldHeight").setFloatingValue(binaryMarkerSize.height);
// Func<Image, Transform3Df, bool> detectFiducialMarker = (Image image, Transform3Df localpose) =>
// {
// Image greyImage = SharedPtr.Alloc<Image>().AddTo(subscriptions);
// Image binaryImage = SharedPtr.Alloc<Image>().AddTo(subscriptions);
// Contour2DfArray contours = new Contour2DfArray().AddTo(subscriptions);
// Contour2DfArray filtered_contours = new Contour2DfArray().AddTo(subscriptions);
// ImageList patches = new ImageList().AddTo(subscriptions);
// Contour2DfArray recognizedContours = new Contour2DfArray().AddTo(subscriptions);
// DescriptorBuffer recognizedPatternsDescriptors = new DescriptorBuffer().AddTo(subscriptions);
// DescriptorBuffer markerPatternDescriptor = new DescriptorBuffer().AddTo(subscriptions);
// DescriptorMatchVector patternMatches = new DescriptorMatchVector().AddTo(subscriptions);
// Point2DfArray pattern2DPoints = new Point2DfArray().AddTo(subscriptions);
// Point2DfArray img2DPoints = new Point2DfArray().AddTo(subscriptions);
// Point3DfArray pattern3DPoints = new Point3DfArray().AddTo(subscriptions);
// bool marker_found = false;
// // Convert Image from RGB to grey
// imageConvertor.convert(image, greyImage, Image.ImageLayout.LAYOUT_GREY).Check();
// // Convert Image from grey to black and white
// imageFilterBinary.filter(greyImage, binaryImage);
// // Extract contours from binary image
// contoursExtractor.extract(binaryImage, contours);
// // Filter 4 edges contours to find those candidate for marker contours
// contoursFilter.filter(contours, filtered_contours);
// // Create one warpped and cropped image by contour
// perspectiveController.correct(binaryImage, filtered_contours, patches);
// // test if this last image is really a squared binary marker, and if it is the case, extract its descriptor
// if (patternDescriptorExtractor.extract(patches, filtered_contours, recognizedPatternsDescriptors, recognizedContours) != FrameworkReturnCode._ERROR_)
// {
// // From extracted squared binary pattern, match the one corresponding to the squared binary marker
// if (patternMatcher.match(markerPatternDescriptor, recognizedPatternsDescriptors, patternMatches) == IDescriptorMatcher.RetCode.DESCRIPTORS_MATCHER_OK)
// {
// // Reindex the pattern to create two vector of points, the first one corresponding to marker corner, the second one corresponding to the poitsn of the contour
// patternReIndexer.reindex(recognizedContours, patternMatches, pattern2DPoints, img2DPoints);
// // Compute the 3D position of each corner of the marker
// img2worldMapper.map(pattern2DPoints, pattern3DPoints);
// // Compute the pose of the camera using a Perspective n Points algorithm using only the 4 corners of the marker
// if (pnp.estimate(img2DPoints, pattern3DPoints, pose) == FrameworkReturnCode._SUCCESS)
// {
// marker_found = true;
// }
// }
// }
// return marker_found;
// };
// if (detectFiducialMarker(camImage, poseFrame1))
// {
// keypointsDetector.detect(camImage, keypointsView1);
// descriptorExtractor.extract(camImage, keypointsView1, descriptorsView1);
// keyframe1 = new Datastructure.Keyframe(keypointsView1, descriptorsView1, camImage, poseFrame1);
// CloudPointVector tmpCP = new CloudPointVector();
// DescriptorMatchVector tmpDMV1 = new DescriptorMatchVector();
// DescriptorMatchVector tmpDMV2 = new DescriptorMatchVector();
// mapper.update(map, keyframe1, tmpCP, tmpDMV1, tmpDMV2);
// keyframePoses.Add(poseFrame1); // used for display
// kfRetriever.addKeyframe(keyframe1); // add keyframe for reloc
// }
// bool bootstrapOk = false;
// while (!bootstrapOk)
// {
// if (camera.getNextImage(view2) == FrameworkReturnCode._ERROR_)
// break;
// if (!detectFiducialMarker(view2, poseFrame2))
// {
// if (imageViewer.display(view2) == FrameworkReturnCode._STOP)
// return FrameworkReturnCode._ERROR_;
// continue;
// }
// float disTwoKeyframes = (float)Math.Sqrt(Math.Pow(poseFrame1.translation().coeff(0, 0) - poseFrame2.translation().coeff(0, 0), 2.0f) + Math.Pow(poseFrame1.translation().coeff(1, 0) - poseFrame2.translation().coeff(1, 0), 2.0f) +
// Math.Pow(poseFrame1.translation().coeff(2, 0) - poseFrame2.translation().coeff(2, 0), 2.0f));
// if (disTwoKeyframes < 0.1)
// {
// if (imageViewer.display(view2) == FrameworkReturnCode._STOP)
// return FrameworkReturnCode._STOP;
// continue;
// }
// keypointsDetector.detect(view2, keypointsView2);
// descriptorExtractor.extract(view2, keypointsView2, descriptorsView2);
// Frame frame2 = new Frame(keypointsView2, descriptorsView2, view2, keyframe1);
// matcher.match(descriptorsView1, descriptorsView2, matches);
// int nbOriginalMatches = matches.Count;
// matchesFilter.filter(matches, matches, keypointsView1, keypointsView2);
// matchesOverlay.draw(view2, imageMatches, keypointsView1, keypointsView2, matches);
// if (imageViewer.display(imageMatches) == FrameworkReturnCode._STOP)
// return FrameworkReturnCode._STOP;
// if (keyframeSelector.select(frame2, matches))
// {
// frame2.setPose(poseFrame2);
// // Triangulate
// keyframe2 = new Datastructure.Keyframe(frame2);
// triangulator.triangulate(keyframe2, matches, cloud);
// //double reproj_error = triangulator->triangulate(keypointsView1, keypointsView2, matches, std::make_pair(0, 1), poseFrame1, poseFrame2, cloud);
// mapFilter.filter(poseFrame1, poseFrame2, cloud, filteredCloud);
// keyframePoses.Add(poseFrame2); // used for display
// DescriptorMatchVector tmpDMV = new DescriptorMatchVector();
// mapper.update(map, keyframe2, filteredCloud, matches, tmpDMV);
// kfRetriever.addKeyframe(keyframe2); // add keyframe for reloc
// if (bundling)
// {
// IntVector firstIdxKFs = new IntVector() { 0, 1 };
// localBundleAdjuster(firstIdxKFs, bundleReprojError);
// }
// bootstrapOk = true;
// }
// }
// Action<Datastructure.Keyframe, CloudPointVector, UIntVector, Datastructure.Keyframe, Frame> updateData =
// (Datastructure.Keyframe refKf, CloudPointVector localMap, UIntVector idxLocalMap, Datastructure.Keyframe referenceKeyframe, Frame frameToTrack) =>
// {
// referenceKeyframe = refKf;
// frameToTrack = new Frame(referenceKeyframe);
// frameToTrack.setReferenceKeyframe(referenceKeyframe);
// idxLocalMap.Clear();
// localMap.Clear();
// mapper.getLocalMapIndex(referenceKeyframe, idxLocalMap);
// foreach (var it in idxLocalMap)
// localMap.Add(mapper.getGlobalMap().getAPoint((int)it));
// };
// // check need to make a new keyframe based on all existed keyframes
// Func<Frame, bool> checkNeedNewKfWithAllKfs = (Frame newFrame)
// =>
// {
// KeyframeList ret_keyframes = new KeyframeList();
// if (kfRetriever.retrieve(newFrame, ret_keyframes) == FrameworkReturnCode._SUCCESS)
// {
// if (ret_keyframes[0].m_idx != referenceKeyframe.m_idx)
// {
// updatedRefKf = ret_keyframes[0];
// return true;
// }
// return false;
// }
// else
// return false;
// };
// // checkDisparityDistance
// Func<Frame, bool> checkDisparityDistance = (Frame newFrame)
// =>
// {
// CloudPointVector cloudPoint = map.getPointCloud();
// KeypointArray refKeypoints = referenceKeyframe.getKeypoints();
// MapIntInt refMapVisibility = referenceKeyframe.getVisibleMapPoints();
// CloudPointVector cpRef = new CloudPointVector();
// Point2DfArray projected2DPts = new Point2DfArray();
// Point2DfArray ref2DPts = new Point2DfArray();
// foreach (var it in refMapVisibility)
// {
// cpRef.Add(cloudPoint[(int)it.Value]);
// ref2DPts.Add(new Point2Df(refKeypoints[(int)it.Key].getX(), refKeypoints[(int)it.Key].getY()));
// }
// projector.project(cpRef, projected2DPts, newFrame.getPose());
// uint imageWidth = newFrame.getView().getWidth();
// double totalMatchesDist = 0.0;
// for (int i = 0; i < projected2DPts.Count; i++)
// {
// Point2Df pt1 = ref2DPts[i];
// Point2Df pt2 = projected2DPts[i];
// totalMatchesDist += Math.Sqrt(((pt1.getX() - pt2.getX()) * (pt1.getX() - pt2.getX())) + ((pt1.getY() - pt2.getY()) * (pt1.getY() - pt2.getY()))) / imageWidth;
// }
// double meanMatchesDist = totalMatchesDist / projected2DPts.Count;
// return (meanMatchesDist > 0.07);
// };
// Action<Datastructure.Keyframe> updateAssociateCloudPoint = (Datastructure.Keyframe newKf) =>
// {
// MapIntInt newkf_mapVisibility = newKf.getVisibleMapPoints();
// MapIntInt kfCounter = new MapIntInt();
// foreach (var it in newkf_mapVisibility)
// {
// CloudPoint cp = map.getAPoint((int)it.Value);
// // calculate the number of connections to other keyframes
// MapIntInt cpKfVisibility = cp.getVisibility();
// foreach (var it_kf in cpKfVisibility)
// kfCounter[it_kf.Value]++;
// ///// update descriptor of cp: des_cp = ((des_cp * cp.getVisibility().size()) + des_buf) / (cp.getVisibility().size() + 1)
// //// TO DO
// cp.visibilityAddKeypoint((uint)newKf.m_idx, it.Key);
// }
// foreach (var it in kfCounter)
// if ((it.Key != newKf.m_idx) && (it.Value > 20))
// {
// newKf.addNeighborKeyframe(it.Key, it.Value);
// }
// };
// Action<Datastructure.Keyframe, IntVector, List<Tuple<uint, int, uint>>, CloudPointVector> findMatchesAndTriangulation =
// (Datastructure.Keyframe newKf, IntVector idxBestNeighborKfs,
// List<Tuple<uint, int, uint>> infoMatches, CloudPointVector cloudPoint) =>
// {
// MapIntInt newKf_mapVisibility = newKf.getVisibleMapPoints();
// DescriptorBuffer newKf_des = newKf.getDescriptors();
// KeypointArray newKf_kp = newKf.getKeypoints();
// Transform3Df newKf_pose = newKf.getPose();
// bool[] checkMatches = new bool[newKf_kp.Capacity];
// for (uint i = 0; i < newKf_kp.Count; ++i)
// if (newKf_mapVisibility[i] != newKf_mapVisibility.Count)
// {
// checkMatches[i] = true;
// }
// for (int i = 0; i < idxBestNeighborKfs.Count; ++i)
// {
// IntVector newKf_indexKeypoints = new IntVector();
// for (int j = 0; j < checkMatches.Length; ++j)
// if (!checkMatches[j])
// newKf_indexKeypoints.Add(j);
// // get neighbor keyframe i
// Datastructure.Keyframe tmpKf = mapper.getKeyframe(idxBestNeighborKfs[i]);
// Transform3Df tmpPose = tmpKf.getPose();
// // check distance between two keyframes
// double distPose = Math.Sqrt(Math.Pow(newKf_pose.translation().coeff(0, 0) - tmpPose.translation().coeff(0, 0), 2.0f) + Math.Pow(newKf_pose.translation().coeff(0, 1)
// - tmpPose.translation().coeff(0, 1), 2.0f) + Math.Pow(newKf_pose.translation().coeff(0, 2) - tmpPose.translation().coeff(0, 2), 2.0f));
// if (distPose < 0.05)
// continue;
// // Matching based on BoW
// DescriptorMatchVector tmpMatches = new DescriptorMatchVector();
// DescriptorMatchVector goodMatches = new DescriptorMatchVector();
// kfRetriever.match(newKf_indexKeypoints, newKf_des, idxBestNeighborKfs[i], tmpMatches);
// // matches filter based epipolar lines
// matchesFilter.filter(tmpMatches, tmpMatches, newKf_kp, tmpKf.getKeypoints(), newKf.getPose(), tmpKf.getPose(), camera.getIntrinsicsParameters());
// // find info to triangulate
// List<Tuple<uint, int, uint>> tmpInfoMatches = new List<Tuple<uint, int, uint>>();
// MapIntInt tmpMapVisibility = tmpKf.getVisibleMapPoints();
// for (int j = 0; j < tmpMatches.Capacity; ++j)
// {
// uint idx_newKf = tmpMatches[j].getIndexInDescriptorA();
// uint idx_tmpKf = tmpMatches[j].getIndexInDescriptorB();
// if ((!checkMatches[idx_newKf]) && !(tmpMapVisibility.Keys.Contains(idx_tmpKf)))
// {
// tmpInfoMatches.Add(new Tuple<uint, int, uint>(idx_newKf, idxBestNeighborKfs[i], idx_tmpKf));
// goodMatches.Add(tmpMatches[j]);
// }
// }
// // triangulation
// CloudPointVector tmpCloudPoint = new CloudPointVector();
// CloudPointVector tmpFilteredCloudPoint = new CloudPointVector();
// IntVector indexFiltered = new IntVector();
// if (goodMatches.Count > 0)
// triangulator.triangulate(newKf_kp, tmpKf.getKeypoints(), newKf_des, tmpKf.getDescriptors(), goodMatches,
// new PairUIntUInt((uint)newKf.m_idx, (uint)idxBestNeighborKfs[i]), newKf.getPose(), tmpKf.getPose(), tmpCloudPoint);
// // filter cloud points
// if (tmpCloudPoint.Count > 0)
// mapFilter.filter(newKf.getPose(), tmpKf.getPose(), tmpCloudPoint, tmpFilteredCloudPoint, indexFiltered);
// for (int o = 0; o < indexFiltered.Count; ++o)
// {
// checkMatches[(tmpInfoMatches[indexFiltered[o]]).Item1] = true;
// infoMatches.Add(tmpInfoMatches[indexFiltered[o]]);
// cloudPoint.Add(tmpFilteredCloudPoint[o]);
// }
// }
// };
// Action<Datastructure.Keyframe, UIntVector, List<Tuple<uint, int, uint>>, CloudPointVector> fuseCloudPoint =
// (Datastructure.Keyframe newKeyframe, UIntVector idxNeigborKfs, List<Tuple<uint, int, uint>> infoMatches, CloudPointVector newCloudPoint)
// =>
// {
// bool[] checkMatches = new bool[newCloudPoint.Capacity];
// for (int i = 0; i < checkMatches.Length; i++)
// checkMatches[i] = true;
// DescriptorBufferList desNewCloudPoint = new DescriptorBufferList();
// foreach (var it_cp in newCloudPoint)
// {
// desNewCloudPoint.Add(it_cp.getDescriptor());
// }
// for (int i = 0; i < idxNeigborKfs.Count; ++i)
// {
// // get a neighbor
// Datastructure.Keyframe neighborKf = mapper.getKeyframe((int)idxNeigborKfs[i]);
// MapIntInt mapVisibilitiesNeighbor = neighborKf.getVisibleMapPoints();
// // projection points
// Point2DfArray projected2DPts = new Point2DfArray();
// projector.project(newCloudPoint, projected2DPts, neighborKf.getPose());
// DescriptorMatchVector allMatches = new DescriptorMatchVector();
// matcher.matchInRegion(projected2DPts, desNewCloudPoint, neighborKf, allMatches, 5.0f);
// for (int j = 0; j < allMatches.Count; ++j)
// {
// int idxNewCloudPoint = (int)allMatches[j].getIndexInDescriptorA();
// int idxKpNeighbor = (int)allMatches[j].getIndexInDescriptorB();
// if (!checkMatches[idxNewCloudPoint])
// continue;
// Tuple<uint, int, uint> infoMatch = infoMatches[idxNewCloudPoint];
// // check this cloud point is created from the same neighbor keyframe
// if (infoMatch.Item2 == idxNeigborKfs[i])
// continue;
// // check if have a cloud point in the neighbor keyframe is coincide with this cloud point.
// bool it_cp = mapVisibilitiesNeighbor.ContainsKey((uint)idxKpNeighbor);
// if (it_cp)
// {
// // fuse
// CloudPoint old_cp = map.getAPoint((int)mapVisibilitiesNeighbor[(uint)idxKpNeighbor]);
// old_cp.visibilityAddKeypoint((uint)newKeyframe.m_idx, infoMatch.Item1);
// old_cp.visibilityAddKeypoint((uint)infoMatch.Item2, infoMatch.Item3);
// newKeyframe.addVisibleMapPoint(infoMatch.Item1, mapVisibilitiesNeighbor[(uint)idxKpNeighbor]);
// mapper.getKeyframe(infoMatch.Item2).addVisibleMapPoint(infoMatch.Item3, mapVisibilitiesNeighbor[(uint)idxKpNeighbor]);
// checkMatches[idxNewCloudPoint] = false;
// }
// }
// }
// List<Tuple<uint, int, uint>> tmpInfoMatches = new List<Tuple<uint, int, uint>>();
// CloudPointVector tmpNewCloudPoint = new CloudPointVector();
// for (int i = 0; i < checkMatches.Length; ++i)
// if (checkMatches[i])
// {
// tmpInfoMatches.Add(infoMatches[i]);
// tmpNewCloudPoint.Add(newCloudPoint[i]);
// }
// infoMatches = tmpInfoMatches;
// newCloudPoint = tmpNewCloudPoint;
// };
// Func<Frame, Frame> processNewKeyframe = (Frame newFrame) =>
// {
// // create a new keyframe from the current frame
// Datastructure.Keyframe newKeyframe = new Datastructure.Keyframe(newFrame);
// // Add to BOW retrieval
// kfRetriever.addKeyframe(newKeyframe);
// // Update keypoint visibility, descriptor in cloud point and connections between new keyframe with other keyframes
// updateAssociateCloudPoint(newKeyframe);
// // get best neighbor keyframes
// UIntVector idxBestNeighborKfs = newKeyframe.getBestNeighborKeyframes(4);
// IntVector tmp_idxBestNeighborKfs = new IntVector();
// for (int i = 0; i < idxBestNeighborKfs.Count; ++i)
// tmp_idxBestNeighborKfs[i] = (int)idxBestNeighborKfs[i];
// // find matches between unmatching keypoints in the new keyframe and the best neighboring keyframes
// List<Tuple<uint, int, uint>> infoMatches = new List<Tuple<uint, int, uint>>(); // first: index of kp in newKf, second: index of Kf, third: index of kp in Kf.
// CloudPointVector newCloudPoint = new CloudPointVector();
// findMatchesAndTriangulation(newKeyframe, tmp_idxBestNeighborKfs, infoMatches, newCloudPoint);
// if (newCloudPoint.Count > 0)
// {
// // fuse duplicate points
// UIntVector idxNeigborKfs = newKeyframe.getBestNeighborKeyframes(10);
// fuseCloudPoint(newKeyframe, idxNeigborKfs, infoMatches, newCloudPoint);
// }
// // mapper update
// mapper.update(map, newKeyframe, newCloudPoint, infoMatches);
// return newKeyframe;
// };
// // Prepare for tracking
// lastPose = poseFrame2;
// updateData(keyframe2, localMap, idxLocalMap, referenceKeyframe, frameToTrack);
// // Get current image
// camera.getNextImage(view);
// keypointsDetector.detect(view, keypointsView);
// descriptorExtractor.extract(view, keypointsView, descriptorsView);
// newFrame = new Frame(keypointsView, descriptorsView, view, referenceKeyframe);
// // match current keypoints with the keypoints of the Keyframe
// matcher.match(frameToTrack.getDescriptors(), descriptorsView, matches);
// matchesFilter.filter(matches, matches, frameToTrack.getKeypoints(), keypointsView);
// Point2DfArray pt2d = new Point2DfArray();
// Point3DfArray pt3d = new Point3DfArray();
// CloudPointVector foundPoints = new CloudPointVector();
// DescriptorMatchVector foundMatches = new DescriptorMatchVector();
// DescriptorMatchVector remainingMatches = new DescriptorMatchVector();
// corr2D3DFinder.find(frameToTrack, newFrame, matches, map, pt3d, pt2d, foundMatches, remainingMatches);
// // display matches
// imageMatches = view.copy();
// Point2DfArray imagePoints_inliers = new Point2DfArray();
// Point3DfArray worldPoints_inliers = new Point3DfArray();
// if (pnpRansac.estimate(pt2d, pt3d, imagePoints_inliers, worldPoints_inliers, newFramePose, lastPose) == FrameworkReturnCode._SUCCESS)
// {
// // Set the pose of the new frame
// newFrame.setPose(newFramePose);
// // refine pose and update map visibility of frame
// {
// // get all keypoints of the new frame
// KeypointArray keypoints = newFrame.getKeypoints();
// // projection points
// Point2DfArray projected2DPts = new Point2DfArray();
// projector.project(localMap, projected2DPts, newFrame.getPose());
// DescriptorBufferList desAllLocalMap = new DescriptorBufferList();
// foreach (var it_cp in localMap)
// {
// desAllLocalMap.Add(it_cp.getDescriptor());
// }
// // matches feature in region
// DescriptorMatchVector allMatches = new DescriptorMatchVector();
// matcher.matchInRegion(projected2DPts, desAllLocalMap, newFrame, allMatches, 5.0f);
// Point2DfArray _pt2d = new Point2DfArray();
// Point3DfArray _pt3d = new Point3DfArray();
// MapIntInt newMapVisibility = new MapIntInt();
// foreach (var it_match in allMatches)
// {
// int idx_2d = (int) it_match.getIndexInDescriptorB();
// int idx_3d = (int) it_match.getIndexInDescriptorA();
// _pt2d.Add(new Point2Df(keypoints[idx_2d].getX(), keypoints[idx_2d].getY()));
// _pt3d.Add(new Point3Df(localMap[idx_3d].getX(), localMap[idx_3d].getY(), localMap[idx_3d].getZ()));
// newMapVisibility[(uint)idx_2d] = idxLocalMap[idx_3d];
// }
// // pnp optimization
// Transform3Df refinedPose = new Transform3Df();
// pnp.estimate(pt2d, pt3d, refinedPose, newFrame.getPose());
// newFrame.setPose(refinedPose);
// // update map visibility of current frame
// newFrame.addVisibleMapPoints(newMapVisibility);
// overlay2D.drawCircles(pt2d, imageMatches);
// overlay3D.draw(refinedPose, imageMatches);
// }
// lastPose = newFrame.getPose();
// // check need new keyframe
// if (checkDisparityDistance(newFrame))
// {
// if (checkNeedNewKfWithAllKfs(newFrame))
// {
// updateData(updatedRefKf, localMap, idxLocalMap, referenceKeyframe, frameToTrack);
// }
// else
// {
// Datastructure.Keyframe newKeyframe = new Datastructure.Keyframe(processNewKeyframe(newFrame));
// if (bundling)
// {
// // get current keyframe idx
// int currentIdxKF = mapper.getKeyframes()[mapper.getKeyframes().Capacity - 1].m_idx;
// // get keyfram connected graph
// UIntVector bestIdx = mapper.getKeyframes()[currentIdxKF].getBestNeighborKeyframes(2);
// // define 2 best keyframes + current keyframe
// windowIdxBundling = new IntVector() {(int)(bestIdx[0]),(int)(bestIdx[1]),currentIdxKF };
// // windowIdxBundling = { currentIdxKF - 1, currentIdxKF }; // temporal sliding window
// localBundleAdjuster(windowIdxBundling, bundleReprojError);
// }
// // update data
// updateData(newKeyframe, localMap, idxLocalMap, referenceKeyframe, frameToTrack);
// // add keyframe pose to display
// keyframePoses.Add(newKeyframe.getPose());
// }
// }
// else
// {
// // update frame to track
// frameToTrack = newFrame;
// }
// framePoses.Add(newFrame.getPose()); // used for display
// isLostTrack = false; // tracking is good
// }
// else
// {
// isLostTrack = true; // lost tracking
// // reloc
// KeyframeList ret_keyframes = new KeyframeList();
// if (kfRetriever.retrieve(newFrame, ret_keyframes) == FrameworkReturnCode._SUCCESS)
// {
// // update data
// updateData(ret_keyframes[0], localMap, idxLocalMap, referenceKeyframe, frameToTrack);
// lastPose = referenceKeyframe.getPose();
// }
// }
// // display matches and a cube on the fiducial marker
// if (imageViewer.display(imageMatches) == FrameworkReturnCode._STOP)
// return FrameworkReturnCode._STOP;
// // display point cloud
// if (viewer3DPoints.display(map.getPointCloud(), lastPose, keyframePoses, framePoses, localMap) == FrameworkReturnCode._STOP)
// return FrameworkReturnCode._STOP;
return(FrameworkReturnCode._SUCCESS);
}
public abstract FrameworkReturnCode Proceed(Image inputImage, Transform3Df pose, ICamera camera);
public virtual FrameworkReturnCode project(CloudPointVector inputPoints, Point2DfArray imagePoints, Transform3Df pose)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_geomPINVOKE.IProject_project__SWIG_2(swigCPtr, CloudPointVector.getCPtr(inputPoints), Point2DfArray.getCPtr(imagePoints), Transform3Df.getCPtr(pose));
if (solar_api_geomPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_geomPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public virtual FrameworkReturnCode estimate(Point2DfArray imagePoints, Point3DfArray worldPoints, Point2DfArray imagePoints_inlier, Point3DfArray worldPoints_inlier, Transform3Df pose)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_solver_posePINVOKE.I3DTransformSACFinderFrom2D3D_estimate__SWIG_1(swigCPtr, Point2DfArray.getCPtr(imagePoints), Point3DfArray.getCPtr(worldPoints), Point2DfArray.getCPtr(imagePoints_inlier), Point3DfArray.getCPtr(worldPoints_inlier), Transform3Df.getCPtr(pose));
if (solar_api_solver_posePINVOKE.SWIGPendingException.Pending)
{
throw solar_api_solver_posePINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
protected override void OnEnable()
{
base.OnEnable();
xpcfComponentManager = xpcf_api.getComponentManagerInstance();
Disposable.Create(xpcfComponentManager.clear).AddTo(subscriptions);
xpcfComponentManager.AddTo(subscriptions);
#if !UN
conf.path = File.ReadAllText("confPath.txt");
#endif
if (xpcfComponentManager.load(conf.path) != XPCFErrorCode._SUCCESS)
{
Debug.LogErrorFormat("Failed to load the configuration file {0}", conf.path);
enabled = false;
return;
}
switch (mode)
{
case PIPELINE.Fiducial:
pipeline = new FiducialPipeline(xpcfComponentManager).AddTo(subscriptions);
break;
case PIPELINE.Natural:
pipeline = new NaturalPipeline(xpcfComponentManager).AddTo(subscriptions);
break;
case PIPELINE.SLAM:
pipeline = new SlamPipeline(xpcfComponentManager).AddTo(subscriptions);
break;
}
overlay3D = xpcfComponentManager.Create("SolAR3DOverlayOpencv").BindTo <I3DOverlay>().AddTo(subscriptions);
switch (source)
{
case SOURCE.SolAR:
camera = xpcfComponentManager.Create("SolARCameraOpencv").BindTo <ICamera>().AddTo(subscriptions);
var intrinsic = camera.getIntrinsicsParameters();
var distorsion = camera.getDistorsionParameters();
var resolution = camera.getResolution();
pipeline.SetCameraParameters(intrinsic, distorsion);
overlay3D.setCameraParameters(intrinsic, distorsion);
OnCalibrate?.Invoke(resolution, intrinsic, distorsion);
if (camera.start() != FrameworkReturnCode._SUCCESS)
{
LOG_ERROR("Camera cannot start");
enabled = false;
}
break;
case SOURCE.Unity:
webcam = new WebCamTexture();
webcam.Play();
if (!webcam.isPlaying)
{
LOG_ERROR("Camera cannot start");
enabled = false;
}
break;
}
switch (display)
{
case DISPLAY.SolAR:
// Set the size of the box to display according to the marker size in world unit
var overlay3D_sizeProp = overlay3D.BindTo <IConfigurable>().getProperty("size");
var size = pipeline.GetMarkerSize();
overlay3D_sizeProp.setFloatingValue(size.width, 0);
overlay3D_sizeProp.setFloatingValue(size.height, 1);
overlay3D_sizeProp.setFloatingValue(size.height / 2.0f, 2);
imageViewer = xpcfComponentManager.Create("SolARImageViewerOpencv").AddTo(subscriptions).BindTo <IImageViewer>().AddTo(subscriptions);
break;
case DISPLAY.Unity:
break;
}
start = clock();
inputImage = SharedPtr.Alloc <Image>().AddTo(subscriptions);
pose = new Transform3Df().AddTo(subscriptions);
}
public virtual FrameworkReturnCode unproject(KeypointArray imageKeypoints, Point3DfArray worldPoints, Transform3Df pose)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_geomPINVOKE.IUnproject_unproject__SWIG_2(swigCPtr, KeypointArray.getCPtr(imageKeypoints), Point3DfArray.getCPtr(worldPoints), Transform3Df.getCPtr(pose));
if (solar_api_geomPINVOKE.SWIGPendingException.Pending)
{
throw solar_api_geomPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
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);
}
public override FrameworkReturnCode Proceed(Image inputImage, Transform3Df pose, ICamera camera)
{
overlay3DComponent.setCameraParameters(camera.getIntrinsicsParameters(), camera.getDistorsionParameters());
// Convert Image from RGB to grey
imageConvertor.convert(inputImage, greyImage, Image.ImageLayout.LAYOUT_GREY).Check();
// Convert Image from grey to black and white
imageFilterBinary.filter(greyImage, binaryImage).Check();
// Extract contours from binary image
contoursExtractor.extract(binaryImage, contours).Check();
#if !NDEBUG
var contoursImage = binaryImage.copy();
overlay2DContours.drawContours(contours, contoursImage);
#endif
// Filter 4 edges contours to find those candidate for marker contours
contoursFilter.filter(contours, filtered_contours).Check();
#if !NDEBUG
var filteredContoursImage = binaryImage.copy();
overlay2DContours.drawContours(filtered_contours, filteredContoursImage);
#endif
// Create one warpped and cropped image by contour
perspectiveController.correct(binaryImage, filtered_contours, patches).Check();
// test if this last image is really a squared binary marker, and if it is the case, extract its descriptor
if (patternDescriptorExtractor.extract(patches, filtered_contours, recognizedPatternsDescriptors, recognizedContours) == FrameworkReturnCode._SUCCESS)
{
#if !NDEBUG
var std__cout = new System.Text.StringBuilder();
/*
* LOG_DEBUG("Looking for the following descriptor:");
* for (var i = 0; i < markerPatternDescriptor.getNbDescriptors() * markerPatternDescriptor.getDescriptorByteSize(); i++)
* {
*
* if (i % patternSize == 0)
* std__cout.Append("[");
* if (i % patternSize != patternSize - 1)
* std__cout.Append(markerPatternDescriptor.data()[i]).Append(", ");
* else
* std__cout.Append(markerPatternDescriptor.data()[i]).Append("]").AppendLine();
* }
* std__cout.AppendLine();
*/
/*
* std__cout.Append(recognizedPatternsDescriptors.getNbDescriptors()).Append(" recognized Pattern Descriptors ").AppendLine();
* uint desrciptorSize = recognizedPatternsDescriptors.getDescriptorByteSize();
* for (uint i = 0; i < recognizedPatternsDescriptors.getNbDescriptors() / 4; i++)
* {
* for (int j = 0; j < patternSize; j++)
* {
* for (int k = 0; k < 4; k++)
* {
* std__cout.Append("[");
* for (int l = 0; l < patternSize; l++)
* {
* std__cout.Append(recognizedPatternsDescriptors.data()[desrciptorSize*((i*4)+k) + j*patternSize + l]);
* if (l != patternSize - 1)
* std__cout.Append(", ");
* }
* std__cout.Append("]");
* }
* std__cout.AppendLine();
* }
* std__cout.AppendLine().AppendLine();
* }
*/
/*
* std__cout.Append(recognizedContours.Count).Append(" Recognized Pattern contour ").AppendLine();
* for (int i = 0; i < recognizedContours.Count / 4; i++)
* {
* for (int j = 0; j < recognizedContours[i].Count; j++)
* {
* for (int k = 0; k < 4; k++)
* {
* std__cout.Append("[").Append(recognizedContours[i * 4 + k][j].getX()).Append(", ").Append(recognizedContours[i * 4 + k][j].getY()).Append("] ");
* }
* std__cout.AppendLine();
* }
* std__cout.AppendLine().AppendLine();
* }
* std__cout.AppendLine();
*/
#endif
// From extracted squared binary pattern, match the one corresponding to the squared binary marker
if (patternMatcher.match(markerPatternDescriptor, recognizedPatternsDescriptors, patternMatches) == Api.Features.IDescriptorMatcher.RetCode.DESCRIPTORS_MATCHER_OK)
{
#if !NDEBUG
std__cout.Append("Matches :").AppendLine();
for (int num_match = 0; num_match < patternMatches.Count; num_match++)
{
std__cout.Append("Match [").Append(patternMatches[num_match].getIndexInDescriptorA()).Append(",").Append(patternMatches[num_match].getIndexInDescriptorB()).Append("], dist = ").Append(patternMatches[num_match].getMatchingScore()).AppendLine();
}
std__cout.AppendLine().AppendLine();
#endif
// Reindex the pattern to create two vector of points, the first one corresponding to marker corner, the second one corresponding to the poitsn of the contour
patternReIndexer.reindex(recognizedContours, patternMatches, pattern2DPoints, img2DPoints).Check();
#if !NDEBUG
LOG_DEBUG("2D Matched points :");
for (int i = 0; i < img2DPoints.Count; i++)
{
LOG_DEBUG("{0}", img2DPoints[i]);
}
for (int i = 0; i < pattern2DPoints.Count; i++)
{
LOG_DEBUG("{0}", pattern2DPoints[i]);
}
overlay2DCircles.drawCircles(img2DPoints, inputImage);
#endif
// Compute the 3D position of each corner of the marker
img2worldMapper.map(pattern2DPoints, pattern3DPoints).Check();
#if !NDEBUG
std__cout.Append("3D Points position:").AppendLine();
for (int i = 0; i < pattern3DPoints.Count; i++)
{
LOG_DEBUG("{0}", pattern3DPoints[i]);
}
#endif
// Compute the pose of the camera using a Perspective n Points algorithm using only the 4 corners of the marker
if (PnP.estimate(img2DPoints, pattern3DPoints, pose) == FrameworkReturnCode._SUCCESS)
{
overlay3DComponent.draw(pose, inputImage);
return(FrameworkReturnCode._SUCCESS);
}
}
#if !NDEBUG
//LOG_DEBUG(std__cout.ToString());
std__cout.Clear();
#endif
}
return(FrameworkReturnCode._ERROR_);
}
public virtual FrameworkReturnCode estimate(KeypointArray pointsView1, KeypointArray pointsView2, Transform3Df poseView1, Transform3Df poseView2, DescriptorMatchVector inlierMatches)
{
FrameworkReturnCode ret = (FrameworkReturnCode)solar_api_solver_posePINVOKE.I3DTransformFinderFrom2D2D_estimate__SWIG_1(swigCPtr, KeypointArray.getCPtr(pointsView1), KeypointArray.getCPtr(pointsView2), Transform3Df.getCPtr(poseView1), Transform3Df.getCPtr(poseView2), DescriptorMatchVector.getCPtr(inlierMatches));
if (solar_api_solver_posePINVOKE.SWIGPendingException.Pending)
{
throw solar_api_solver_posePINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
