/// <summary>
/// Initialization
/// </summary>
public void InitializeBusinessLogic()
{
ConfigurationHelper.RegisterSettings(Properties.Settings.Default);
pointCloudViewModel = new PointcloudViewModel(new SharpDX.Size2(0, 0));
PointCloudView.ViewModel = pointCloudViewModel;
PointCloudView.InitializeScene();
DataContext = pointCloudViewModel;
// Moving Camera vertically up and turning it to look down
PointCloudView.ViewModel.SetCvmPosition(new System.Windows.Media.Media3D.Vector3D(0, 0, groundZ),
new System.Windows.Media.Media3D.Vector3D(180, 0, 90));
var planeColor = System.Windows.Media.Color.FromArgb(20, 50, 255, 50).ToColor4();
// Only points from this rectangular area are included and outliers are ignored
var scenePlane = PointcloudViewModel.CreatePlane(planeColor, groundD, groundW, new SharpDX.Vector3(0, 0, 1f));
PointCloudView.SceneRoot.Children.Add(scenePlane);
// For 3D texts
BillboardTextModel3D text = new BillboardTextModel3D();
PointCloudView.SceneRoot.Children.Add(text);
labels = new BillboardText3D();
text.Geometry = labels;
boundingBoxGroup = new GroupModel3D();
PointCloudView.SceneRoot.Children.Add(boundingBoxGroup);
rosControlBase.RosConnected += RosControlBase_RosConnected;
rosControlBase.RosDisconnected += RosControlBase_RosDisconnected;
rosControlBase.CvmDeviceInfoChaged += RosControlBase_CvmDeviceInfoChaged;
}
/// <summary>
/// Event handler is called when CVM device info has changed
/// </summary>
/// <param name="sender">Sender</param>
/// <param name="e">Arguments</param>
private void RosControlBase_CvmDeviceInfoChaged(object sender, EventArgs e)
{
double f = rosControlBase.Device.DeviceInfo.FocalPoint;
double B = rosControlBase.Device.DeviceInfo.Baseline;
if (pointCloudViewModel.ImagingPipeline != null)
{
pointCloudViewModel.ImagingPipeline.SetCameraInfo(B, f, rosControlBase.Device.DeviceInfo.PrincipalPoint);
}
PointCloudView.AddFov(rosControlBase.Device.DeviceInfo.FovV, rosControlBase.Device.DeviceInfo.FovH, 1f, 10f, rosControlBase.Device.DeviceInfo.Baseline);
}
private void Init()
{
InitializeComponent();
Log.InitLog(textBox, label_Cycle);
// Initialize images
_processingStage = new ProcessingStage(label_Status,
new BitmapImage(new Uri("pack://application:,,,/3DReconstructionWPF;component/Assets/Images/icons8-crossmark.png")),
new BitmapImage(new Uri("pack://application:,,,/3DReconstructionWPF;component/Assets/Images/icons8-checkmark.png")),
image_trackedFeature, image_rgbColor, image_depth);
_renderer = new Renderer(group);
_pcv = new PointCloudView(_renderer);
_sensor = KinectSensor.GetDefault();
_initialTransformation = new pointmatcher.net.EuclideanTransform
{
translation = System.Numerics.Vector3.Zero,
rotation = System.Numerics.Quaternion.Normalize(System.Numerics.Quaternion.CreateFromRotationMatrix(new System.Numerics.Matrix4x4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
)))
};
_icpData = new ICP.ICPData(null, _initialTransformation);
_icp = new ICP();
label_Cycle.Content = "cycle: " + _cycleRuns;
if (_sensor != null)
{
if (_sensor.IsOpen && _sensor.IsAvailable)
{
Log.WriteLog("Kinect capture data available!");
}
}
// Init filters
FilterGroup.InitFilters();
}
/// <summary>
/// Event handler is called when successfully connected to ROS master server
/// </summary>
/// <param name="sender">Sender</param>
/// <param name="e">Arguments</param>
private void RosControlBase_RosConnected(object sender, EventArgs e)
{
try
{
// NOTE: GPU filters can be initialized only when View3D of HelixToolkit has been launched.
PointCloudView.InitializePipeline(Properties.Settings.Default.ForceCpuFiltering, 1);
if (rosControlBase.Device.DeviceInfo != null)
{
pointCloudViewModel.ImagingPipeline.SetCameraInfo(rosControlBase.Device.DeviceInfo.Baseline,
rosControlBase.Device.DeviceInfo.FocalPoint, rosControlBase.Device.DeviceInfo.PrincipalPoint);
}
pointCloudViewModel.ImagingPipeline.ImageDataProcessed += ImagingPipeline_ImageDataProcessed;
}
catch (Exception ex)
{
MessageBox.Show($"Fatal error initializing pipeline: {ex.GetBaseException().Message}");
throw;
}
// RGBPreview.Subscribe();
// DisparityPreview.Subscribe();
}