bool StepDetector::pushAndDetect(IMUData &accelData) {
TIMESTAMP_T t = accelData.time();
IMUDATA_T magnitude = sqrt(accelData.x() * accelData.x() + accelData.y() * accelData.y() + accelData.z() * accelData.z());
if (!Adir_ && magnitude > Aref_)
{
Aref_ = magnitude;
prev_time_ = t;
}
else if (!Adir_ && Aref_ - magnitude >= parameter_ptr->step_delta_mag_thresh())
{
Adir_ = !Adir_;
Aref_ = magnitude;
}
else if(Adir_ && magnitude < Aref_)
{
Aref_ = magnitude;
}
else if(Adir_ && magnitude - Aref_ >= parameter_ptr->step_delta_mag_thresh())
{
if (t - prev_time_ >= parameter_ptr->step_delta_t_thresh()) {
Adir_ = !Adir_;
Aref_ = magnitude;
prev_time_ = t;
return true;
}
}
return false;
}
BaseSensorDataNode* SyncSensorDataNodeMaker::makeNode(MapManager* manager, BaseSensorData* data) {
SynchronizedSensorData* sdata = dynamic_cast<SynchronizedSensorData*>(data);
if (! sdata)
return 0;
SyncSensorDataNode* snode = new SyncSensorDataNode(manager, sdata);
for (size_t i = 0; i<sdata->sensorDatas.size(); i++) {
IMUData* imu = dynamic_cast<IMUData*>(sdata->sensorDatas[i]);
if (imu) {
MapNodeUnaryRelation* imuRel = new MapNodeUnaryRelation(manager);
imuRel->nodes()[0] = snode;
Eigen::Isometry3d t;
t.setIdentity();
t.linear() = imu->orientation().toRotationMatrix();
Eigen::Matrix<double, 6, 6> info;
info.setZero();
info.block<3,3>(3,3) = imu->orientationCovariance().inverse();
//info.block<3,3>(3,3).setIdentity();
imuRel->setTransform(t);
imuRel->setInformationMatrix(info);
snode->setImu(imuRel);
break;
}
}
return snode;
}
/**
Updates robot position information using IMU data
**/
int RobotPosition::update(IMUData newData)
{
double time = newData.time();
_Roll.push(DataPoint<double>(newData.Roll, time));
_Pitch.push(DataPoint<double>(newData.Pitch, time));
_Yaw.push(DataPoint<double>(newData.Yaw, time));
return 0;
}