Skip to content

Commit

Permalink
Add timestamp; remove orienation alignment
Browse files Browse the repository at this point in the history
  • Loading branch information
@simlrh
simlrh committed Sep 27, 2016
1 parent ad75a20 commit 39ad2b2
Show file tree
Hide file tree
Showing 8 changed files with 50 additions and 115 deletions.
53 changes: 0 additions & 53 deletions FusionMath.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,57 +26,4 @@ namespace je_nourish_fusion {
osvrQuatSetW(quaternion, cos(r / 2) * cos(p / 2) * cos(y / 2) + sin(r / 2) * sin(p / 2) * sin(y / 2));
}

void invertQuaternion(OSVR_Quaternion* quaternion) {
double x = osvrQuatGetX(quaternion);
double y = osvrQuatGetY(quaternion);
double z = osvrQuatGetZ(quaternion);
double w = osvrQuatGetW(quaternion);

double magnitude = sqrt(x*x + y*y + z*z + w*w);

osvrQuatSetX(quaternion, -osvrQuatGetX(quaternion) / magnitude);
osvrQuatSetY(quaternion, -osvrQuatGetY(quaternion) / magnitude);
osvrQuatSetY(quaternion, -osvrQuatGetZ(quaternion) / magnitude);

osvrQuatSetW(quaternion, osvrQuatGetW(quaternion) / magnitude);
}

void multiplyQuaternion(OSVR_Quaternion* q1, OSVR_Quaternion* q2) {
double x = osvrQuatGetX(q1) * osvrQuatGetW(q2) + osvrQuatGetY(q1) * osvrQuatGetZ(q2) - osvrQuatGetZ(q1) * osvrQuatGetY(q2) + osvrQuatGetW(q1) * osvrQuatGetX(q2);
double y = -osvrQuatGetX(q1) * osvrQuatGetZ(q2) + osvrQuatGetY(q1) * osvrQuatGetW(q2) + osvrQuatGetZ(q1) * osvrQuatGetX(q2) + osvrQuatGetW(q1) * osvrQuatGetY(q2);
double z = osvrQuatGetX(q1) * osvrQuatGetY(q2) - osvrQuatGetY(q1) * osvrQuatGetX(q2) + osvrQuatGetZ(q1) * osvrQuatGetW(q2) + osvrQuatGetW(q1) * osvrQuatGetZ(q2);
double w = -osvrQuatGetX(q1) * osvrQuatGetX(q2) - osvrQuatGetY(q1) * osvrQuatGetY(q2) - osvrQuatGetZ(q1) * osvrQuatGetZ(q2) + osvrQuatGetW(q1) * osvrQuatGetW(q2);

osvrQuatSetX(q2, x);
osvrQuatSetY(q2, y);
osvrQuatSetZ(q2, z);
osvrQuatSetW(q2, w);
}

void offsetTranslation(OSVR_Vec3* translation_offset, OSVR_Vec3* translation) {
osvrVec3SetX(translation, osvrVec3GetX(translation) - osvrVec3GetX(translation_offset));
osvrVec3SetY(translation, osvrVec3GetY(translation) - osvrVec3GetY(translation_offset));
osvrVec3SetZ(translation, osvrVec3GetZ(translation) - osvrVec3GetZ(translation_offset));
}

void setupAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState) {
OSVR_Vec3* translation = &(poseState->translation);
osvrVec3SetX(&(offset->translation), osvrVec3GetX(translation));
osvrVec3SetY(&(offset->translation), osvrVec3GetY(translation));
osvrVec3SetZ(&(offset->translation), osvrVec3GetZ(translation));

OSVR_Quaternion* rotation = &(poseState->rotation);
osvrQuatSetX(&(offset->rotation), osvrQuatGetX(rotation));
osvrQuatSetY(&(offset->rotation), osvrQuatGetY(rotation));
osvrQuatSetZ(&(offset->rotation), osvrQuatGetZ(rotation));
osvrQuatSetW(&(offset->rotation), osvrQuatGetW(rotation));

invertQuaternion(&(offset->rotation));
}

void applyAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState) {
offsetTranslation(&(offset->translation), &(poseState->translation));
multiplyQuaternion(&(offset->rotation), &(poseState->rotation));
}

}
8 changes: 0 additions & 8 deletions FusionMath.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,4 @@ namespace je_nourish_fusion {
void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy);
void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion);

void invertQuaternion(OSVR_Quaternion* quaternion);
void multiplyQuaternion(OSVR_Quaternion* q1, OSVR_Quaternion* q2);

void offsetTranslation(OSVR_Vec3* translation_offset, OSVR_Vec3* translation);

void setupAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState);
void applyAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState);

}
16 changes: 6 additions & 10 deletions OrientationReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,8 @@ namespace je_nourish_fusion {
SingleOrientationReader::SingleOrientationReader(OSVR_ClientContext ctx, std::string orientation_path) {
osvrClientGetInterface(ctx, orientation_path.c_str(), &m_orientation);
}
OSVR_ReturnCode SingleOrientationReader::update(OSVR_OrientationState* orientation) {
OSVR_TimeValue timestamp;

return osvrGetOrientationState(m_orientation, &timestamp, orientation);
OSVR_ReturnCode SingleOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) {
return osvrGetOrientationState(m_orientation, timeValue, orientation);
}

CombinedOrientationReader::CombinedOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) {
Expand All @@ -31,16 +29,14 @@ namespace je_nourish_fusion {
osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2]));
}

OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation) {
OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) {
OSVR_OrientationState orientation_x;
OSVR_OrientationState orientation_y;
OSVR_OrientationState orientation_z;

OSVR_TimeValue timestamp;

OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], &timestamp, &orientation_x);
OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], &timestamp, &orientation_y);
OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], &timestamp, &orientation_z);
OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], timeValue, &orientation_x);
OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y);
OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z);

OSVR_Vec3 rpy_x;
OSVR_Vec3 rpy_y;
Expand Down
6 changes: 3 additions & 3 deletions OrientationReader.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ namespace je_nourish_fusion {

class IOrientationReader {
public:
virtual OSVR_ReturnCode update(OSVR_OrientationState* orientation) = 0;
virtual OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) = 0;
};

class OrientationReaderFactory {
Expand All @@ -15,15 +15,15 @@ namespace je_nourish_fusion {
class SingleOrientationReader : public IOrientationReader {
public:
SingleOrientationReader(OSVR_ClientContext ctx, std::string orientation_path);
OSVR_ReturnCode update(OSVR_OrientationState* orientation);
OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue);
protected:
OSVR_ClientInterface m_orientation;
};

class CombinedOrientationReader : public IOrientationReader {
public:
CombinedOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths);
OSVR_ReturnCode update(OSVR_OrientationState* orientation);
OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue);
protected:
OSVR_ClientInterface m_orientations[3];
};
Expand Down
16 changes: 6 additions & 10 deletions PositionReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,27 +18,23 @@ namespace je_nourish_fusion {
SinglePositionReader::SinglePositionReader(OSVR_ClientContext ctx, std::string position_path) {
osvrClientGetInterface(ctx, position_path.c_str(), &m_position);
}
OSVR_ReturnCode SinglePositionReader::update(OSVR_PositionState* position) {
OSVR_TimeValue timestamp;

return osvrGetPositionState(m_position, &timestamp, position);
OSVR_ReturnCode SinglePositionReader::update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) {
return osvrGetPositionState(m_position, timeValue, position);
}

CombinedPositionReader::CombinedPositionReader(OSVR_ClientContext ctx, Json::Value position_paths) {
osvrClientGetInterface(ctx, position_paths["x"].asCString(), &(m_positions[0]));
osvrClientGetInterface(ctx, position_paths["y"].asCString(), &(m_positions[1]));
osvrClientGetInterface(ctx, position_paths["z"].asCString(), &(m_positions[2]));
}
OSVR_ReturnCode CombinedPositionReader::update(OSVR_PositionState* position) {
OSVR_ReturnCode CombinedPositionReader::update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) {
OSVR_PositionState position_x;
OSVR_PositionState position_y;
OSVR_PositionState position_z;

OSVR_TimeValue timestamp;

OSVR_ReturnCode xret = osvrGetPositionState(m_positions[0], &timestamp, &position_x);
OSVR_ReturnCode yret = osvrGetPositionState(m_positions[1], &timestamp, &position_y);
OSVR_ReturnCode zret = osvrGetPositionState(m_positions[2], &timestamp, &position_z);
OSVR_ReturnCode xret = osvrGetPositionState(m_positions[0], timeValue, &position_x);
OSVR_ReturnCode yret = osvrGetPositionState(m_positions[1], timeValue, &position_y);
OSVR_ReturnCode zret = osvrGetPositionState(m_positions[2], timeValue, &position_z);

if (xret == OSVR_RETURN_SUCCESS) {
osvrVec3SetX(position, osvrVec3GetX(&position_x));
Expand Down
6 changes: 3 additions & 3 deletions PositionReader.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ namespace je_nourish_fusion {

class IPositionReader {
public:
virtual OSVR_ReturnCode update(OSVR_PositionState* position) = 0;
virtual OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) = 0;
};

class PositionReaderFactory {
Expand All @@ -15,15 +15,15 @@ namespace je_nourish_fusion {
class SinglePositionReader : public IPositionReader {
public:
SinglePositionReader(OSVR_ClientContext ctx, std::string position_path);
OSVR_ReturnCode update(OSVR_PositionState* position);
OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue);
protected:
OSVR_ClientInterface m_position;
};

class CombinedPositionReader : public IPositionReader {
public:
CombinedPositionReader(OSVR_ClientContext ctx, Json::Value position_paths);
OSVR_ReturnCode update(OSVR_PositionState* position);
OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue);
protected:
OSVR_ClientInterface m_positions[3];
};
Expand Down
18 changes: 14 additions & 4 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,17 @@ It can also combine axes from different trackers, eg taking pitch and roll from

Build following the [standard OSVR plugin build instructions](http://resource.osvr.com/docs/OSVR-Core/TopicWritingDevicePlugin.html).

Sample osvr_server_config.json:
## Tracker alignment

The orientation and position data will likely be misaligned, eg, you are facing forward and leaning forward, but your tracked position instead moves to the side. To correct this, align the orientation tracker with the position tracker's axes and run osvr_reset_yaw on the orientation tracker.

For example, with the OSVR HDK and a Kinect, you would place the HDK in front of the Kinect, pointing towards it, then run

osvr_reset_yaw.exe --path "/com_osvr_Multiserver/OSVRHackerDevKitPrediction0/semantic/hmd"

This replaces the `alignInitialOrientation` option in previous versions.

## Sample osvr_server_config.json:

{
"drivers": [
Expand All @@ -18,14 +28,14 @@ Sample osvr_server_config.json:
"name": "DK1_Kinectv2",
"position": "/je_nourish_kinectv2/KinectV2/semantic/body1/head",
"orientation": "/je_nourish_openhmd/Oculus Rift (Devkit)/semantic/hmd",
// Align DK1 and Kinect axes (point hmd directly at kinect on startup)
"alignInitialOrientation": true,
// Eyes are above and in front of the center of the head
"offsetFromRotationCenter": {
"x": 0,
"y": 0.01,
"z": -0.05
}
},
// Pass the timestamp from the Kinect skeleton data to OSVR
"timestamp": "position"
}
},
// Combine more accurate pitch and roll from Wii Nunchuk with yaw and position from Kinect
Expand Down
42 changes: 18 additions & 24 deletions je_nourish_fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,10 @@ namespace je_nourish_fusion {
class FusionDevice {
public:
FusionDevice(OSVR_PluginRegContext ctx, Json::Value config) {
std::cout << "Constructing fusion device" << std::endl;

m_firstUpdate = true;
osvrPose3SetIdentity(&m_state);

m_useAlign = config.isMember("alignInitialOrientation")
&& config["alignInitialOrientation"].asBool();
m_useTimestamp = config.isMember("timestamp");
m_usePositionTimestamp = m_useTimestamp && config["timestamp"].asString().compare("position") == 0;

if ((m_useOffset = config.isMember("offsetFromRotationCenter"))) {
osvrVec3Zero(&m_offset);
Expand Down Expand Up @@ -42,31 +39,24 @@ namespace je_nourish_fusion {
m_orientationReader = OrientationReaderFactory::getReader(m_ctx, config["orientation"]);

if (m_positionReader == NULL) {
std::cout << "Position Reader not created" << std::endl;
std::cout << "Fusion Device: Position Reader not created" << std::endl;
}
if (m_orientationReader == NULL) {
std::cout << "Orientation Reader not created" << std::endl;
std::cout << "Fusion Device: Orientation Reader not created" << std::endl;
}

m_dev->sendJsonDescriptor(je_nourish_fusion_json);
m_dev->registerUpdateCallback(this);
}

OSVR_ReturnCode update() {

osvrClientUpdate(m_ctx);

m_positionReader->update(&m_state.translation);
m_orientationReader->update(&m_state.rotation);

OSVR_TimeValue timeValuePosition;
OSVR_TimeValue timeValueOrientation;

if (m_useAlign) {
if (m_firstUpdate) {
m_firstUpdate = false;
setupAlign(&m_align, &m_state);
}
applyAlign(&m_align, &m_state);
}
m_positionReader->update(&m_state.translation, &timeValuePosition);
m_orientationReader->update(&m_state.rotation, &timeValueOrientation);

if (m_useOffset) {
Eigen::Quaterniond rotation = osvr::util::fromQuat(m_state.rotation);
Expand All @@ -75,7 +65,13 @@ namespace je_nourish_fusion {
translation += rotation._transformVector(osvr::util::vecMap(m_offset));
}

osvrDeviceTrackerSendPose(*m_dev, m_tracker, &m_state, 0);
if (m_useTimestamp) {
OSVR_TimeValue timeValue = m_usePositionTimestamp ? timeValuePosition : timeValueOrientation;
osvrDeviceTrackerSendPoseTimestamped(*m_dev, m_tracker, &m_state, 0, &timeValue);
}
else {
osvrDeviceTrackerSendPose(*m_dev, m_tracker, &m_state, 0);
}

return OSVR_RETURN_SUCCESS;
}
Expand All @@ -92,13 +88,11 @@ namespace je_nourish_fusion {
OSVR_TrackerDeviceInterface m_tracker;
OSVR_PoseState m_state;

bool m_firstUpdate;

bool m_useAlign;
OSVR_PoseState m_align;

bool m_useOffset;
OSVR_Vec3 m_offset;

bool m_useTimestamp;
bool m_usePositionTimestamp;
};

class FusionDeviceConstructor {
Expand Down

0 comments on commit 39ad2b2

Please sign in to comment.