Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

consistent notation #116

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

consistent notation #116

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
434045b
changed usage of q_wv to comply with its notation. changed notation o…
otim Jun 8, 2015
d3057d5
replaced ? : with if else structure
otim Jun 8, 2015
84b094d
addressed simon's comments
otim Jun 8, 2015
bd39f9e
remove conjugate for consistent notation
otim Jun 12, 2015
58b2363
in position_sensormanager it is p_wp not p_vc
otim Jun 12, 2015
e18218a
renamed q_vc to q_pose because attitude measurements can be either q_…
otim Jun 22, 2015
c3b05a9
Merge branch 'master' into fix/consistent_notation
otim Jun 22, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 21 additions & 21 deletions msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ struct PoseMeasurement : public PoseMeasurementBase {
// Get rotation matrices.
Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>()
.toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>()
Eigen::Matrix<double, 3, 3> C_wi = state.Get<StateDefinition_T::q>()
.toRotationMatrix();

Eigen::Matrix<double, 3, 3> C_ci = state.Get<StateQicIdx>()
Expand All @@ -175,7 +175,7 @@ struct PoseMeasurement : public PoseMeasurementBase {
Eigen::Matrix<double, 3, 1> vecold;

vecold = (-state.Get<StatePwvIdx>() + state.Get<StateDefinition_T::p>()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you move this up s.t. declaration is definition?

+ C_q * state.Get<StatePicIdx>()) * state.Get<StateLIdx>();
+ C_wi * state.Get<StatePicIdx>()) * state.Get<StateLIdx>();
Eigen::Matrix<double, 3, 3> skewold = Skew(vecold);

Eigen::Matrix<double, 3, 3> pci_sk = Skew(state.Get<StatePicIdx>());
Expand Down Expand Up @@ -224,27 +224,27 @@ struct PoseMeasurement : public PoseMeasurementBase {

// Construct H matrix.
// Position:
H.block<3, 3>(0, kIdxstartcorr_p) = C_wv
H.block<3, 3>(0, kIdxstartcorr_p) = C_wv.transpose()
* state.Get<StateLIdx>()(0); // p

H.block<3, 3>(0, kIdxstartcorr_q) = -C_wv * C_q * pci_sk
H.block<3, 3>(0, kIdxstartcorr_q) = -C_wv.transpose() * C_wi * pci_sk
* state.Get<StateLIdx>()(0); // q

H.block<3, 1>(0, kIdxstartcorr_L) =
scalefix ?
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you also fix this horrible ? : syntax ?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if you do so, please also change below:

if (scalefix) {
  H.block<3, 1>(0, kIdxstartcorr_L).setZero();
} else {
  H.block<3, 1>(0, kIdxstartcorr_L) = ...;
}

Eigen::Matrix<double, 3, 1>::Zero() :
(C_wv * C_q * state.Get<StatePicIdx>() + C_wv
(C_wv.transpose() * C_wi * state.Get<StatePicIdx>() + C_wv.transpose()
* (-state.Get<StatePwvIdx>()
+ state.Get<StateDefinition_T::p>())).eval(); // L

H.block<3, 3>(0, kIdxstartcorr_qwv) =
driftwvattfix ?
Eigen::Matrix<double, 3, 3>::Zero() : (-C_wv * skewold).eval(); // q_wv
Eigen::Matrix<double, 3, 3>::Zero() : (-C_wv.transpose() * skewold).eval(); // q_wv

H.block<3, 3>(0, kIdxstartcorr_pic) =
calibposfix ?
Eigen::Matrix<double, 3, 3>::Zero() :
(C_wv * C_q * state.Get<StateLIdx>()(0)).eval(); //p_ic
(C_wv.transpose() * C_wi * state.Get<StateLIdx>()(0)).eval(); //p_ic


// TODO (slynen): Check scale commenting
Expand All @@ -260,7 +260,7 @@ struct PoseMeasurement : public PoseMeasurementBase {
H.block<3, 3>(3, kIdxstartcorr_qwv) =
driftwvattfix ?
Eigen::Matrix<double, 3, 3>::Zero() :
(C_ci * C_q.transpose()).eval(); // q_wv
(C_ci * C_wi.transpose()).eval(); // q_wv

H.block<3, 3>(3, kIdxstartcorr_qic) =
calibattfix ?
Expand Down Expand Up @@ -295,22 +295,22 @@ struct PoseMeasurement : public PoseMeasurementBase {
// Get rotation matrices.
Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>()

.conjugate().toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>()
.conjugate().toRotationMatrix();
.toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_wi = state.Get<StateDefinition_T::q>()
.toRotationMatrix();

// Construct residuals.
// Position.
r_old.block<3, 1>(0, 0) = z_p_
- (C_wv.transpose()
- (C_wv
* (-state.Get<StatePwvIdx>()
+ state.Get<StateDefinition_T::p>()
+ C_q.transpose() * state.Get<StatePicIdx>()))
+ C_wi * state.Get<StatePicIdx>()))
* state.Get<StateLIdx>();

// Attitude.
Eigen::Quaternion<double> q_err;
q_err = (state.Get<StateQwvIdx>()
q_err = (state.Get<StateQwvIdx>().conjugate()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you add a comment that this means:

q_err = \hat{q}_vc * q_cv

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

isn't here （calculate the residual of position）? p_vc = c_vw*(-p_wv+p_wi + c_wi*pic)L = c_vw(pw_vi + pw_ic)L = c_vw(pw_vc)L = p_vcL (pw_vi means p_vi expressed in the world frame)
is c_wv means the rotation matrix from vision frame to world frame ?
so why use the “C_wv” but not the "C_wv.transpose()" in line 305 ,what's more in function "CalculateH( ) " use" H.block<3, 3>(0, kIdxstartcorr_p) = C_wv.transpose( )" for calculation of the Jacobian

* state.Get<StateDefinition_T::q>()
* state.Get<StateQicIdx>()).conjugate() * z_q_;
r_old.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2;
Expand Down Expand Up @@ -388,24 +388,24 @@ struct PoseMeasurement : public PoseMeasurementBase {

//TODO (slynen): check that both measurements have the same states fixed!
Eigen::Matrix<double, 3, 3> C_wv_old, C_wv_new;
Eigen::Matrix<double, 3, 3> C_q_old, C_q_new;
Eigen::Matrix<double, 3, 3> C_wi_old, C_wi_new;

C_wv_new = state_new.Get<StateQwvIdx>().conjugate().toRotationMatrix();
C_q_new = state_new.Get<StateDefinition_T::q>().conjugate()
C_wv_new = state_new.Get<StateQwvIdx>().toRotationMatrix();
C_wi_new = state_new.Get<StateDefinition_T::q>()
.toRotationMatrix();

C_wv_old = state_old.Get<StateQwvIdx>().conjugate().toRotationMatrix();
C_q_old = state_old.Get<StateDefinition_T::q>().conjugate()
C_wv_old = state_old.Get<StateQwvIdx>().toRotationMatrix();
C_wi_old = state_old.Get<StateDefinition_T::q>()
.toRotationMatrix();

// Construct residuals.
// Position:
Eigen::Matrix<double, 3, 1> diffprobpos = (C_wv_new.transpose()
* (-state_new.Get<StatePwvIdx>() + state_new.Get<StateDefinition_T::p>()
+ C_q_new.transpose() * state_new.Get<StatePicIdx>()))
+ C_wi_new * state_new.Get<StatePicIdx>()))
* state_new.Get<StateLIdx>() - (C_wv_old.transpose()
* (-state_old.Get<StatePwvIdx>() + state_old.Get<StateDefinition_T::p>()
+ C_q_old.transpose() * state_old.Get<StatePicIdx>()))
+ C_wi_old * state_old.Get<StatePicIdx>()))
* state_old.Get<StateLIdx>();


Expand Down
14 changes: 7 additions & 7 deletions msf_updates/src/pose_msf/pose_sensormanager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,7 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<

void Init(double scale) const {
Eigen::Matrix<double, 3, 1> p, v, b_w, b_a, g, w_m, a_m, p_ic, p_vc, p_wv;
Eigen::Quaternion<double> q, q_wv, q_ic, q_cv;
Eigen::Quaternion<double> q, q_wv, q_ic, q_vc;
msf_core::MSF_Core<EKFState_T>::ErrorStateCov P;

// init values
Expand All @@ -170,16 +170,16 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<
P.setZero(); // Error state covariance; if zero, a default initialization in msf_core is used

p_vc = pose_handler_->GetPositionMeasurement();
q_cv = pose_handler_->GetAttitudeMeasurement();
q_vc = pose_handler_->GetAttitudeMeasurement();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are you sure about this? At least vicon publishes p_vc and q_cv

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

looks like we need to measurement_world_sensor parameter here

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry about that! For me this was the case, so this notation made it easier to understand for me... Simon, can you please elaborate on your idea?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Depending on the subscribed topic (ptam/vicon) the pose provided can be
either q_cv (ptam) or q_vc/q_wc (vicon). If you look at the implementation
of the pose update you see that there is a fix-parameter (from yaml) that
defines if the pose measurement needs to be inverted or not before being
applied as update. You would need to add the same logic here.

On Tue, Jun 16, 2015, 09:39 Tim Oberhauser [email protected] wrote:

In msf_updates/src/pose_msf/pose_sensormanager.h
#116 (comment):

@@ -170,16 +170,16 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<
P.setZero(); // Error state covariance; if zero, a default initialization in msf_core is used

 p_vc = pose_handler_->GetPositionMeasurement();
  • q_cv = pose_handler_->GetAttitudeMeasurement();
  • q_vc = pose_handler_->GetAttitudeMeasurement();

Sorry about that! For me this was the case, so this notation made it
easier to understand for me... Simon, can you please elaborate on your idea?

—
Reply to this email directly or view it on GitHub
https://github.com/ethz-asl/ethzasl_msf/pull/116/files#r32497243.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

please have a look at my latest commit

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I understand correctly, the first attitude measurement was always added inversed in the case of vicon... Ok, vicon measurements are added at a high rate so the effect was maybe never visible...

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here it says that measurement_world_sensor is false for ptam:
http://wiki.ros.org/ethzasl_sensor_fusion/Tutorials/sfly_framework


MSF_INFO_STREAM(
"initial measurement pos:["<<p_vc.transpose()<<"] orientation: "<<STREAMQUAT(q_cv));
"initial measurement pos:["<<p_vc.transpose()<<"] orientation: "<<STREAMQUAT(q_vc));

// Check if we have already input from the measurement sensor.
if (p_vc.norm() == 0)
MSF_WARN_STREAM(
"No measurements received yet to initialize position - using [0 0 0]");
if (q_cv.w() == 1)
if (q_vc.w() == 1)
MSF_WARN_STREAM(
"No measurements received yet to initialize attitude - using [1 0 0 0]");

Expand All @@ -195,14 +195,14 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<
q_ic.normalize();

// Calculate initial attitude and position based on sensor measurements.
if (q_cv.w() == 1) { // If there is no pose measurement, only apply q_wv.
if (q_vc.w() == 1) { // If there is no pose measurement, only apply q_wv.
q = q_wv;
} else { // If there is a pose measurement, apply q_ic and q_wv to get initial attitude.
q = (q_ic * q_cv.conjugate() * q_wv).conjugate();
q = (q_ic * q_vc.conjugate() * q_wv.conjugate()).conjugate();
}

q.normalize();
p = p_wv + q_wv.conjugate().toRotationMatrix() * p_vc / scale
p = p_wv + q_wv.toRotationMatrix() * p_vc / scale
- q.toRotationMatrix() * p_ic;

a_m = q.inverse() * g; /// Initial acceleration.
Expand Down
4 changes: 2 additions & 2 deletions msf_updates/src/pose_pressure_msf/pose_pressure_sensormanager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,10 +161,10 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
if (q_vc.w() == 1) { // If there is no pose measurement, only apply q_wv.
q = q_wv;
} else { // If there is a pose measurement, apply q_ic and q_wv to get initial attitude.
q = (q_ic * q_vc.conjugate() * q_wv).conjugate();
q = (q_ic * q_vc.conjugate() * q_wv.conjugate()).conjugate();
}
q.normalize();
p = q_wv.conjugate().toRotationMatrix() * p_vc / scale
p = q_wv.toRotationMatrix() * p_vc / scale
- q.toRotationMatrix() * p_ic;

a_m = q.inverse() * g; /// Initial acceleration.
Expand Down
2 changes: 1 addition & 1 deletion msf_updates/src/position_pose_msf/position_pose_sensormanager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -192,7 +192,7 @@ class PositionPoseSensorManager : public msf_core::MSF_SensorManagerROS<
MSF_WARN_STREAM("q " << STREAMQUAT(q));
MSF_WARN_STREAM("q_wv " << STREAMQUAT(q_wv));

Eigen::Matrix<double, 3, 1> p_vision = q_wv.conjugate().toRotationMatrix()
Eigen::Matrix<double, 3, 1> p_vision = q_wv.toRotationMatrix()
* p_vc / scale - q.toRotationMatrix() * p_ic;

//TODO (slynen): what if there is no initial position measurement? Then we
Expand Down