state.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <stephan dot weiss at ieee dot org>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include <ssf_core/state.h>
00033 
00034 namespace ssf_core
00035 {
00036 
00037 void State::reset(){
00038   // states varying during propagation
00039   p_.setZero();
00040   v_.setZero();
00041   q_.setIdentity();
00042   b_w_.setZero();
00043   b_a_.setZero();
00044 
00045   L_ = 1.0;
00046   q_wv_.setIdentity();
00047   q_ci_.setIdentity();
00048   p_ci_.setZero();
00049 
00050   w_m_.setZero();
00051   a_m_.setZero();
00052 
00053   q_int_.setIdentity();
00054 
00055   P_.setZero();
00056   time_ = 0;
00057 }
00058 
00059 void State::getPoseCovariance(geometry_msgs::PoseWithCovariance::_covariance_type & cov)
00060 {
00061   assert(cov.size() == 36);
00062 
00063   for (int i = 0; i < 9; i++)
00064     cov[i / 3 * 6 + i % 3] = P_(i / 3 * N_STATE + i % 3);
00065 
00066   for (int i = 0; i < 9; i++)
00067     cov[i / 3 * 6 + (i % 3 + 3)] = P_(i / 3 * N_STATE + (i % 3 + 6));
00068 
00069   for (int i = 0; i < 9; i++)
00070     cov[(i / 3 + 3) * 6 + i % 3] = P_((i / 3 + 6) * N_STATE + i % 3);
00071 
00072   for (int i = 0; i < 9; i++)
00073     cov[(i / 3 + 3) * 6 + (i % 3 + 3)] = P_((i / 3 + 6) * N_STATE + (i % 3 + 6));
00074 }
00075 
00076 void State::toPoseMsg(geometry_msgs::PoseWithCovarianceStamped & pose)
00077 {
00078   eigen_conversions::vector3dToPoint(p_, pose.pose.pose.position);
00079   eigen_conversions::quaternionToMsg(q_, pose.pose.pose.orientation);
00080   getPoseCovariance(pose.pose.covariance);
00081 }
00082 
00083 void State::toExtStateMsg(sensor_fusion_comm::ExtState & state)
00084 {
00085   eigen_conversions::vector3dToPoint(p_, state.pose.position);
00086   eigen_conversions::quaternionToMsg(q_, state.pose.orientation);
00087   eigen_conversions::vector3dToPoint(v_, state.velocity);
00088 }
00089 
00090 void State::toStateMsg(sensor_fusion_comm::DoubleArrayStamped & state)
00091 {
00092   state.data[0] = p_[0];
00093   state.data[1] = p_[1];
00094   state.data[2] = p_[2];
00095   state.data[3] = v_[0];
00096   state.data[4] = v_[1];
00097   state.data[5] = v_[2];
00098   state.data[6] = q_.w();
00099   state.data[7] = q_.x();
00100   state.data[8] = q_.y();
00101   state.data[9] = q_.z();
00102   state.data[10] = b_w_[0];
00103   state.data[11] = b_w_[1];
00104   state.data[12] = b_w_[2];
00105   state.data[13] = b_a_[0];
00106   state.data[14] = b_a_[1];
00107   state.data[15] = b_a_[2];
00108   state.data[16] = L_;
00109   state.data[17] = q_wv_.w();
00110   state.data[18] = q_wv_.x();
00111   state.data[19] = q_wv_.y();
00112   state.data[20] = q_wv_.z();
00113   state.data[21] = q_ci_.w();
00114   state.data[22] = q_ci_.x();
00115   state.data[23] = q_ci_.y();
00116   state.data[24] = q_ci_.z();
00117   state.data[25] = p_ci_[0];
00118   state.data[26] = p_ci_[1];
00119   state.data[27] = p_ci_[2];
00120 }
00121 
00122 }; // end namespace ssf_core


ssf_core
Author(s): Stephan Weiss, Markus Achtelik
autogenerated on Fri Jan 3 2014 11:21:29