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;
}