Tune Phidget sensor fusion parameters about pluslib HOT 1 OPEN

2013-05-17 10:54
Andras Lasso
The issue that they have is that currently none of the sensor fusion algorithms in PLUS work well enough for this relatively simple task:
the TiltSensor mode doesn't do any sensor fusion or filtering therefore it's very noisy
the OrientationSensor mode with magnetometer (AHRS mode) is not reliable due to inhomogeneous magnetic field
the OrientationSensor mode without magnetometer (IMU mode) has rotation drift (around the "down" axis, as there is no compass and the measurement of the heading only relies on gyroscopes)

What should work: use the IMU algorithm but constrain the rotation around the "initial down direction" to 0 (we would request that they move the transducer to horizontal position when they turn on the system, that would be the initial down direction). We may need to tune the sensor fusion parameters as well. Need to verify the results with optical tracker measurements.

2013-05-17 11:05
Andras Lasso
@fedorov: Could you attach the header file of a few typical sequences? (so that we can determine the optimal parameters for that rotation speed)

2013-05-17 12:09
Andras Lasso
Magnetometer may not be reliable and may require frequent re-calibration. How to prevent drift when no magnetometer is used?

In vtkPhidgetSpatialTracker::SpatialDataHandler":

Option A: change the gyroscope input (before calling tracker->AhrsAlgo->UpdateIMU(...)) of the sensor fusion algorithm (basically force the measured rotation speed to 0 along the down axis)

Option B: change the output of the sensor fusion algorithm (after calling tracker->AhrsAlgo->UpdateIMU(...)) to snap the rotation around the down axis to 0 (e.g., represent the orientation with Euler angles in the south-west-down coordinate system and set the rotation value around the down axis to 0).

Probably option B would be simpler and more robust (with option A, some rotations around the down axis could still accumulate as a result of numerical inaccuracies).

2013-05-17 15:52
Andrey Fedorov
Attachment TrackedImageSequence_20121102_114233_scan-converted.mhd added
file:bBjGakVYSr4ORdacwqEsg8

2013-05-17 15:52
Andrey Fedorov
TrackedImageSequence_20121102_114233_scan-converted.mhd.txt
TrackedImageSequence_EpiphanCaptureDevice_20130412_084805_scan_converted_OutputChannel_BKVideoStream.mhd.txt

2013-05-17 15:55
Andrey Fedorov
@lassoan: examples uploaded. Regarding optical tracker, today Isaiah and I calibrated Plus with NDI Polaris. We can use this setup to compare the two trackers as well (if I can get the stepper and motor from rad onc!)

2013-05-17 16:03
Andras Lasso
@fedorov: Thanks for the sequences. It would be great if you could acquire sequences with simultaneous PhidgetSpatial and Polaris tracking data - see the example config file below.

<DataCollection StartupDelaySec="1.0" >

<DeviceSet Name="NDI Polaris + PhidgetSpatial" Description="NDI Polaris + PhidgetSpatial tracking" />

<Device Id="AllTrackerOutputDevice" Type="VirtualMixer">
<InputChannels>
<InputChannel Id="MargTrackerStream" />
<InputChannel Id="OpticalTrackerStream" />
</InputChannels>
<OutputChannels>
<OutputChannel Id="AllTrackerStream" />
</OutputChannels>
</Device>

<Device
Id="OpticalTrackerDevice"
Type="PolarisTracker"
SerialPort="8"
BaudRate="115200"
AcquisitionRate="50"
LocalTimeOffsetSec="0.0" >
<DataSources>
<DataSource Type="Tool" Id="OpticalMarker" PortName="4" RomFile="..\NdiToolDefinitions\8700339.rom" AveragedItemsForFiltering="20" BufferSize="1500"/>
<DataSource Type="Tool" Id="Reference" PortName="6" RomFile="..\NdiToolDefinitions\8700449.rom" AveragedItemsForFiltering="20" BufferSize="1500"/>
</DataSources>
<OutputChannels>
<OutputChannel Id="OpticalTrackerStream" >
<DataSource Id="OpticalMarker"/>
<DataSource Id="Reference"/>
</OutputChannel>
</OutputChannels>
</Device>

<Device
Id="MargTrackerDevice"
Type="PhidgetSpatial"
AhrsAlgorithm="MADGWICK_IMU"
AhrsAlgorithmGain="0.05"
TiltSensorWestAxisIndex="1"
AcquisitionRate="125"
LocalTimeOffsetSec="0.0">
<DataSources>
<DataSource Type="Tool" Id="Accelerometer" PortName="Accelerometer" BufferSize="1500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="Gyroscope" PortName="Gyroscope" BufferSize="1500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="Magnetometer" PortName="Magnetometer" BufferSize="1500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="TiltSensor" PortName="TiltSensor" BufferSize="1500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="OrientationSensor" PortName="OrientationSensor" BufferSize="1500" AveragedItemsForFiltering="20"/>
</DataSources>
<OutputChannels>
<OutputChannel Id="MargTrackerStream" >
<DataSource Id="Accelerometer"/>
<DataSource Id="Gyroscope"/>
<DataSource Id="Magnetometer"/>
<DataSource Id="TiltSensor"/>
<DataSource Id="OrientationSensor"/>
</OutputChannel>
</OutputChannels>
</Device>

</DataCollection>

2013-05-20 10:34
Andrey Fedorov
I will try to book the equipment and do this on Friday.

I did the reconstructions from the data collected last week, and it does not look good. It's very noisy. I will try to repeat this with the phantom fixed in place.

In your experience, is there a constraint how quickly the probe can move with NDI?

2013-05-20 10:42
Andras Lasso
Which data is noisy: the Phidget or the NDI tracker? Or the image data? Which NDI tracker do you use? With active or passive markers? NDI Polaris is not particularly sensitive to speed.

2013-05-20 10:47
Andrey Fedorov

Which data is noisy: the Phidget or the NDI tracker?

Both :) But in this case I was talking about reconstruction from NDI only, the setup we used last week with the phantom was NDI + Epiphan. The setup was somewhat suboptimal (I had to start recording while holding the phantom and the probe, and the phantom was not rigidly attached to the table), so I will try again on Friday.

Or the image data? Which NDI tracker do you use?

We used NDI Polaris tracker that came with a Hologic US/CT fusion system.

<Device
Id="TrackerDevice"
Type="PolarisTracker"
SerialPort="5"
BaudRate="115200"
AcquisitionRate="50"
LocalTimeOffsetSec="0">

With active or passive markers?

I believe these are passive markers -- snap-on shiny balls.

2013-05-20 12:36
Andras Lasso
The Polaris with passive markers should be quite accurate, the error should be a few tenth of a millimeters. I didn't experience noise pose measurements with free-hand motion. If you used the motorized rotator and the noise of the pose measurement was only high during motion then it might be related to some vibration.

2013-05-23 09:32
Andrey Fedorov
@lassoan: is there a tool that would allow to explore different fusion parameters/algorithms for a stored dataset?

You mentioned using fusion algorithm with/without magnetometer, but I have never tried without magnetometer -- AhrsAlgorithm="MADGWICK_MARG" in acquisition config file. I only tried TiltSensor and OrientationSensor from the saved sequence file captured with these settings.

It would be nice to have a standalone tool so I can specify algorithm in config file, and it would produce an extra transform based on algorithm/parameters in a saved sequence file. If there is no such tool, I can try to make one. Please let me know if you or @Anderson_PERK have it.

2013-05-23 10:19
Andrey Fedorov
@lassoan:

1. can you clarify what you mean by the following: "The angularRate parameters should be updated before calling tracker->AhrsAlgo->UpdateIMU(...) in vtkPhidgetSpatialTracker::SpatialDataHandler" ?

2. Phidget manual describes a procedure to calibrate magnetometer for the specific environment, otherwise the measurements may not be useful: http://www.phidgets.com/docs/1056_User_Guide#Magnetic_Error_Correction_.28Calibration.29 Is there a reason you think this is not necessary?

2013-05-23 10:42
Andras Lasso

It would be nice to have a standalone tool so I can specify algorithm in config file, and it would produce an extra transform based on algorithm/parameters in a saved sequence file. If there is no such tool, I can try to make one.

We don't have a standalone tool for recomputing the sensor fusion results from raw data. It would be nice if you could create such a tool.

The angularRate parameters should be updated before calling tracker->AhrsAlgo->UpdateIMU(...) in vtkPhidgetSpatialTracker::SpatialDataHandler

This is a note to Ryan about how to prevent drift when no magnetometer is used. Option A (that is described in the note): change the gyroscope input of the sensor fusion algorithm (basically force the measured rotation speed to 0 along the down axis). Option B: change the output of the sensor fusion algorithm to snap the rotation around the down axis to 0 (e.g., represent the orientation with Euler angles in the south-west-down coordinate system and set the rotation value around the down axis to 0). Probably option B would be simpler.

Phidget manual describes a procedure to calibrate magnetometer for the specific environment... Is there a reason you think this is not necessary?

It could work, however you may need to perform the calibration before each procedure (which may be difficult, because it requires rotation along all axes, while the transducer motion is constrained by the stabilizer) and it's not guaranteed that the calibration can fully compensate the presence of all distortion. As we need rotation angle only along one axis (which is not the down axis), we should be able to get that angle without relying on a magnetometer - with one of the options described above.

2013-05-23 11:02
Andrey Fedorov
Andras, another problem with magnetometer is that the sensor is located relatively close to the motor.

I looked at the measurements from the gyroscope alone, and the angular rate for the second axis seems quite stable, see plot attached (mean 3.5 deg/sec stdev 0.24 for the slow portion of the sweep). Do you think a simple solution that uses this single measurement with perhaps some averaging over consecutive frames could be sufficiently good?

2013-05-23 11:03
Andrey Fedorov

example angular rate around second axis measured from Phidget 3/3/3 gyroscope sensor

The Y axis title is incorrect -- this is angular rate deg/sec ! sorry

2013-05-23 11:42
Andras Lasso
The drift of the angle value is probably negligible during a single acquisition, so you can integrate the angular velocity to get rotation angle value. Make sure you use all the 3 sensor directions, if the sensor axis is not aligned exactly with the transducer rotation axis.

2013-05-23 11:57
Andrey Fedorov
re all 3 sensor directions, not sure, since the signal is very noisy for the other two. mean/std is 17 for the primary one, and ~1 for the other two. The sensor is roughly in good alignment with the rotation axis, since we have a fixed position attachment adapter.

2013-05-23 12:37
Andras Lasso
I guess stdev is the same for all gyro axes. If the alignment is perfect then you have non-zero mean on one axis and zero mean on the two others.

2013-05-24 16:53
Andrey Fedorov
You are right, I don't know what I was thinking ...

Regarding optical tracking together with Phidget: I will first work to design an attachment to allow consistent placement of the star. Calibration procedure for the transrectal probe is very cumbersome, and currently has to be repeated every time. I think we can improve this. I was working on another sub-project with US today, and decided not to spend time on joint tracking until I have a robust system.

2013-05-28 15:47
Andras Lasso

• Reproduce drfiting accelerometer measurements (move the accel while zeroing, if needed)
• Test if the updated algo stops the rotation around the down axis
• Option A: constrain by decomposing to rotation around SWD axes and setting D rotation component to 0 (assume that initial orientation of the tracked object is SWD). Option B: assume that one of the sensor's axes always points to W (similar to tilt sensor).
• Find a good name for the new coordinate system (RelativeTiltSensor for Option A, FilteredTiltSensor for Option B)

2013-05-29 16:43
Andrey Fedorov
(In plus:2819) re #675: adding a tool to recover transformation that corresponds to angular position based on Phidget gyroscope reading, with the possibility to filter frames based on angular rate threshold

2013-05-30 13:06
Ryan Anderson
(In plus:2822) re #675: Added constrain to the OrientationSensor tool (WIP)

2013-05-30 16:27
Ryan Anderson
(In plus:2824) re #675: Cleaned up code with constraint added to Orientation Sensor tool, new tool called Filtered Tilt Sensor.

2013-05-30 16:56
Ryan Anderson

<PlusConfiguration version="2.1">

<DataCollection StartupDelaySec="1.0">
<DeviceSet
Name="Phidget Spatial test"
Description="Configuration file just PhidgetSpatial tracking. Keep the device stationary for 2 seconds after connect."
/>
<Device
Id="TrackerDevice"
Type="PhidgetSpatial"
AhrsAlgorithm="MADGWICK_MARG"
AhrsAlgorithmGain="1.5"
FilteredTiltAhrsAlgorithm="MADGWICK_MARG"
FilteredTiltAhrsAlgorithmGain="1.5"
TiltSensorWestAxisIndex="1"
FilteredTiltSensorWestAxisIndex="1"
AcquisitionRate="125"
LocalTimeOffsetSec="0.0" >
<DataSources>
<DataSource Type="Tool" Id="OrientationSensor" PortName="OrientationSensor" BufferSize="2500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="TiltSensor" PortName="TiltSensor" BufferSize="2500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="FilteredTiltSensor" PortName="FilteredTiltSensor" BufferSize="2500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="Accelerometer" PortName="Accelerometer" BufferSize="2500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="Gyroscope" PortName="Gyroscope" BufferSize="2500" AveragedItemsForFiltering="20"/>
<DataSource Type="Tool" Id="Magnetometer" PortName="Magnetometer" BufferSize="2500" AveragedItemsForFiltering="20"/>
</DataSources>

<OutputChannels>
<OutputChannel Id="TrackerStream" >
<DataSource Id="OrientationSensor"/>
<DataSource Id="TiltSensor"/>
<DataSource Id="FilteredTiltSensor"/>
<DataSource Id="Accelerometer"/>
<DataSource Id="Gyroscope"/>
<DataSource Id="Magnetometer"/>
</OutputChannel>
</OutputChannels>
</Device>
</DataCollection>

<PlusOpenIGTLinkServer
MaxNumberOfIgtlMessagesToSend="1"
MaxTimeSpentWithProcessingMs="50"
ListeningPort="18944"
OutputChannelId="TrackerStream"
>
<DefaultClientInfo>
<MessageTypes>
<Message Type="TRANSFORM" />
</MessageTypes>
<TransformNames>
<Transform Name="OrientationSensorToTracker" />
<Transform Name="TiltSensorToTracker" />
<Transform Name="FilteredTiltSensorToTracker" />
<Transform Name="AccelerometerToTracker" />
<Transform Name="GyroscopeToTracker" />
<Transform Name="MagnetometerToTracker" />
</TransformNames>
</DefaultClientInfo>
</PlusOpenIGTLinkServer>

<VolumeReconstruction OutputSpacing="0.5 0.5 0.5"
ClipRectangleOrigin="0 0" ClipRectangleSize="820 616"
Interpolation="LINEAR" Optimization="FULL" Compounding="On" FillHoles="Off"
/>

</PlusConfiguration>

2013-05-30 17:15
Ryan Anderson
The above config file works with the the updated filtered tilt sensor.
The AHRS algorithm and algorithm parameters can be changed independently for the tilt sensor and filtered tilt sensor.

2013-05-30 20:28
Andras Lasso
@fedorov: We've completed the sensor fusion implementation for the tilt sensor. Use the PortName="FilteredTiltSensor" to get a drift-free (no rotation drift around the down axis) and reduced-noise orientation matrix. Set these parameters:
FilteredTiltAhrsAlgorithm="MADGWICK_IMU" => you can also try MAHONY_IMU
FilteredTiltAhrsAlgorithmGain="1.5" => you can try to change this parameter (it balances between noise suppression and quick convergence to the accelerometer-measured orientation)
FilteredTiltSensorWestAxisIndex="1" => specify here the axis number (0, 1, or 2) of the tilt sensor axis that is parallel to the rotation axis (points towards the tip of the transducer)

Ideally, this should be tested with the motorized stepper and with the optical tracker as ground truth, but if you don't have access to the motorized stepper then it's enough if you can just do some manual testing.

2013-05-30 22:21
Andrey Fedorov
I will test. I don't know if I will be able to do this tomorrow. I have few critical deadlines, and may need to postpone.

Also, Andras - regarding my earlier question if the filter can be applied to previously recorded data. I just thought can't it be done by specifying the recorded sequence file as data source and replaying it?

2013-05-30 23:42
Andras Lasso
Currently the sensor fusion algorithm files are only used privately by the PhidgetSpatial class, so we need to reorganize the code to make it available to Plus applications. Then the tool to re-computate sensor fusion results from recorded raw measurement data can be implemented. Probably it's better to create a new tool for that, very similar to the existing PhidgetVolumeReconstruction tool, we could call it SpatialSensorFusion.

I'll do this code reorganization tomorrow (Friday) => #770
@Anderson_PERK: after the code reorganization is ready, please implement this SpatialSensorFusion tool (you'll need this for your work anyway) => #771

2013-06-28 11:38
Andrey Fedorov
re #675: add missing quotes to fix compile error
Committed to: plus
In plus:2879

2013-06-28 16:00
Andrey Fedorov
refs #675,766: adding BWH config file that has Epiphan and both Polaris and Epiphan trackers configured
Committed to: plus
In plus:2888

2013-06-28 16:03
Andrey Fedorov
I was not able to test this today, since the stepper is used by clinical staff, but the config file is ready, so I will try to test next week, maybe Tuesday - I have the config file with optical and phidget trackers ready

2013-07-02 20:04
Andrey Fedorov
TrackedImageSequence_20130702_125545_config.xml.txt
Polaris + Phidget config file

2013-07-02 20:05
Andrey Fedorov
TrackedImageSequence_20130702_125545_header.mha.txt
Polaris + Phidget tracked image file header

2013-07-02 20:05
Andrey Fedorov
I did the experiment today and attached the dataset to the ticket.

2013-07-04 09:51
Andras Lasso

Tilt sensor angle measurements vs. ground truth optical

2013-07-04 09:52
Andras Lasso

Orientation sensor (sensor fusion, with compass) angle measurements vs. ground truth optical

2013-07-04 09:53
Andras Lasso

Filtered tilt sensor (sensor fusion, without compass) angle measurements vs. ground truth optical

2013-07-04 10:19
Andras Lasso
@Andriy,

A SpatialSensorFusion tool (see Diagnostic_and_test_tools page) was created to allow post-processing of raw sensor data to get quite noise-free but still accurate orientation. The computed orientation is stored in the FilteredTiltSensorToPhidgetTracker transform.

Example for obtaining filtered tilt sensor data from raw accelerometer+gyroscope data:

set PLUS_BIN_DIR=c:/Users/lasso/devel/PlusExperimental-bin/bin/Release
set DATA_DIR=c:/Users/lasso/devel/PocketUsTracking/trunk/data/Phidget Data/20130702-BwhPhidgetOptical/

%PLUS_BIN_DIR%/SpatialSensorFusion.exe --ahrs-algo=MADGWICK_IMU --ahrs-algo-gain 0.01 --initial-gain 1 --initial-repeated-frame-number=1000 --input-seq-file="%DATA_DIR%/TransrectalProbeRotatedPhidgetPolaris.mha" --output-seq-file="%DATA_DIR%/TransrectalProbeRotatedPhidgetPolarisReprocessed.mha" --west-axis-index=1 --tracker-reference-frame=PhidgetTracker

Based on the off-line reprocessing, the 0.01 gain value seemed to be the best (low noise, still accurate angles compared to the optical ground truth) for the filtered tilt sensor. See the results in attached charts above (comparing the orientation sensor, tilt sensor, and filtered tilt sensor tools to the ground truth optical tracker tool)

The FilteredTiltSensor tool is available also in real-time during acquisition. It would be great if you could double-check that the this tool gives optimal result during acquisition as well, with the same 0.01 gain value (during acquisition we have access to the full frame rate of the PhidgetSpatial sensor, so there may be slight differences).

See PlusConfiguration_NoVideo_Phidget.xml config file for an example for adding the FilteredTiltSensor tool to your config file (https://www.assembla.com/code/bm1A8eCNSr4l2deJe5cbCb/bnyqsQCNSr4l2deJe5cbCb/commit/2914).

2013-07-12 09:20
Andrey Fedorov
@lassoan, where is the source code for SpatialSensorFusion tool? Do I need to enable Phidget to compile it?

I do not see either source code, or binary:

[fedorov@gridftp-spl PlusBuild2-bin]$ ls
bin CMakeCache.txt DartConfiguration.tcl itk-prefix OpenIGTLink-bin PlusApp-bin PlusBuildAndTest.sh PlusLib-prefix vtk-bin
BuildAndTest.bat CMakeFiles itk Makefile OpenIGTLink-prefix PlusApp-prefix PlusLib Testing vtk-prefix
BuildAndTest.sh cmake_install.cmake itk-bin OpenIGTLink PlusApp PlusBuildAndTest.bat PlusLib-bin vtk
[fedorov@gridftp-spl PlusBuild2-bin]$ find . |grep SpatialSensorFusion
./PlusLib/data/TestImages/SpatialSensorFusionTestInput.mha
./PlusLib/data/TestImages/SpatialSensorFusionTestBaseline.mha
./PlusLib/data/TestImages/.svn/text-base/SpatialSensorFusionTestInput.mha.svn-base
./PlusLib/data/TestImages/.svn/text-base/SpatialSensorFusionTestBaseline.mha.svn-base
[fedorov@gridftp-spl PlusBuild2-bin]$

Trying to enable Phidget on my Linux machine (this is where I keep all the data and do the processing) leads to this error:

CMake Error at src/DataCollection/CMakeLists.txt:688 (FILE):
file COPY cannot find
"/xnat/fedorov/local/PLUS/PlusBuild2-bin/PlusLib/tools/Phidget/PhidgetSpatial-2.1.8/x86/phidget21.so".

Indeed, there is only .lib file for Phidget, no shared lib.

2013-07-12 09:53
Andras Lasso
The sensor fusion tool is in PlusApp. No dependency on Phidget or vica versa.

2013-07-12 09:58
Andrey Fedorov
Thank you, resolved

2013-07-16 10:05
Andrey Fedorov
re #675, use absolute values of angular rate for filtering
Committed to: plus
In plus:2943

2013-07-16 10:08
Andrey Fedorov
Gyroscope_vs_FilteredSensorFusion_reconstruction.pptx
Comparison of FilteredSensorFusion and single axis Gyroscope angular rate based volume reconstruction

2013-07-16 10:10
Andrey Fedorov
@lassoan, I uploaded a document summarizing some observations comparing sensor fusion and gyroscope based reconstruction. I will continue using both for the remaining datasets, and will report if I have more updates.

2013-07-16 13:09
Andras Lasso
Thanks Andriy, this is interesting. Could you attach the image header (no pixel data needed), the config file used for the acquisition and the volume reconstruction?

2013-07-17 14:53
Andrey Fedorov
Case23_recon_debug.zip
Header and config files used for acquisition/reconstruction

2013-07-17 14:54
Andrey Fedorov
Done.

Acquisition config: TrackedImageSequence_BKCaptureDevice_20130412_083827_config.xml
Gyroscope reconstruction config: TrackedImageSequence_BKCaptureDevice_20130412_083827_GyroFiltered_Recon_config.xml
Gyroscope filtered header: TrackedImageSequence_BKCaptureDevice_20130412_083827_ScanConverted_GyroFiltered.mhd
Sensor fusion recon header: TrackedImageSequence_BKCaptureDevice_20130412_083827_ScanConverted_SensorFusionFiltered.mhd
Sensor fusion recon config: TrackedImageSequence_BKCaptureDevice_20130412_083827_SensorFusionFiltered_Recon_config.xml

2013-07-17 19:01
Andrey Fedorov
I processed 11 cases today, and in all cases gyroscope based reconstruction was visually superior to sensor fusion. There appears to be some drift remaining. I will next attach another example where it is quite prominent. Same settings were used for sensor fusion in all cases.

There were some problems with gyroscope sensor measurements in some cases, but maybe I will report those separately. The good thing is I think I have 11 relatively good volumes for the US/MR study, and 2 more I need to look a bit more, because there were some issues with the config file structure (those were collected in the times of transitioning to multi-channel setup).

2013-07-17 19:02
Andrey Fedorov

another example of drift with the sensor fusion based reconstruction

2013-12-05 18:05
Andras Lasso
Milestone changed from Release Plus-2.1.0 to Release Plus-Future

from pluslib.