diff --git a/README.md b/README.md
index 85b1049601cc35b21e38c0d7b8efa6176a2ece17..d79fb3e945d7b59b1d84c558ef5c8d5bfa9f44b7 100644
--- a/README.md
+++ b/README.md
@@ -110,16 +110,16 @@ libglog.so will be installed at **/usr/local/lib**
- Git clone the source:
- $ git clone https://ceres-solver.googlesource.com/ceres-solver
-
+ $ git clone https://ceres-solver.googlesource.com/ceres-solver
+
- Build and install with:
- $ cd ceres-solver
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
- $Â make
- $ sudo make installÂ
+ $ cd ceres-solver
+ $ mkdir build
+ $ cd build
+ $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
+ $ make
+ $ sudo make install
libceres.a will be installed at **/usr/local/lib** and headers at **/usr/local/include/ceres**
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 9adb719715a0188f8c55a50d5d4443b92f829645..152595422b16a653d70bfa7600d6fe0a62b305b0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -207,7 +207,7 @@ SET(HDRS_BASE
map_base.h
motion_buffer.h
node_base.h
- pinholeTools.h
+ pinhole_tools.h
problem.h
processor_base.h
processor_capture_holder.h
diff --git a/src/IMU_tools.h b/src/IMU_tools.h
index 98f895282487655fe76d56f6f6ccd593f24656c8..ebdf64df1a37dfcc945996c93368657abf11dd47 100644
--- a/src/IMU_tools.h
+++ b/src/IMU_tools.h
@@ -220,8 +220,8 @@ inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, co
MatrixSizeCheck<3, 1>::check(diff_v);
diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
- diff_q = dq1.conjugate() * dq2;
diff_v = dq1.conjugate() * ( dv2 - dv1 );
+ diff_q = dq1.conjugate() * dq2;
}
template<typename D1, typename D2, typename D3, class T>
@@ -307,8 +307,8 @@ inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1
MatrixSizeCheck<3, 1>::check(dv);
dp = q1.conjugate() * ( p2 - p1 - v1*dt - (T)0.5*gravity().cast<T>()*(T)dt*(T)dt );
- dq = q1.conjugate() * q2;
dv = q1.conjugate() * ( v2 - v1 - gravity().cast<T>()*(T)dt );
+ dq = q1.conjugate() * q2;
}
template<typename D1, typename D2, typename D3, class T>
@@ -359,8 +359,8 @@ Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) );
dp_ret = dp_in;
- do_ret = log_q(dq_in);
dv_ret = dv_in;
+ do_ret = log_q(dq_in);
return ret;
}
@@ -380,8 +380,8 @@ Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) );
dp = dp_in;
- dq = exp_q(do_in);
dv = dv_in;
+ dq = exp_q(do_in);
return ret;
}
@@ -392,8 +392,8 @@ inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v )
{
plus_p = dp1 + dp2;
- plus_q = dq1 * exp_q(do2);
plus_v = dv1 + dv2;
+ plus_q = dq1 * exp_q(do2);
}
template<typename D1, typename D2, typename D3>
@@ -429,8 +429,8 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const
MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v )
{
diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
diff_v = dv2 - dv1;
+ diff_o = log_q(dq1.conjugate() * dq2);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
@@ -485,7 +485,6 @@ inline void diff(const MatrixBase<D1>& d1,
diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
-
/* d = diff(d1, d2) is
* dp = dp2 - dp1
* do = Log(dq1.conj * dq2)
@@ -501,7 +500,6 @@ inline void diff(const MatrixBase<D1>& d1,
J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
-
}
template<typename D1, typename D2>
@@ -528,8 +526,8 @@ inline void body2delta(const MatrixBase<D1>& a,
MatrixSizeCheck<3,1>::check(dv);
dp = 0.5 * a * dt * dt;
- dq = exp_q(w * dt);
dv = a * dt;
+ dq = exp_q(w * dt);
}
template<typename D1>
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 21d38df4b8b1cc1825d5990095f9af5a581c35b8..b4271a1a09bc6b4085acba5995706c7826fdcdf8 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -90,6 +90,10 @@ void CaptureBase::remove()
{
feature_list_.front()->remove(); // remove downstream
}
+ while (!constrained_by_list_.empty())
+ {
+ constrained_by_list_.front()->remove(); // remove constrained by
+ }
}
}
@@ -146,7 +150,7 @@ void CaptureBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = state_block_vec_[i];
if (sbp != nullptr)
{
if (getProblem() != nullptr)
diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index d70ddd065cff349169ba5550cd873c7ded081cd4..6cd28295eaff3e86c72ee6f3cd679e1430dbf92e 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -5,8 +5,8 @@
#include "constraint_autodiff.h"
#include "landmark_AHP.h"
#include "sensor_camera.h"
-#include "pinholeTools.h"
#include "feature_point_image.h"
+#include "pinhole_tools.h"
#include <iomanip> //setprecision
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index df530b78cdc310c4ec427159d79fd380b142e6ac..30e1db1531de9c8aec3e48ac13cefd33e2328a51 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -62,7 +62,7 @@ void ConstraintBase::remove()
if (getProblem() != nullptr)
getProblem()->removeConstraintPtr(shared_from_this());
- // remove other: {Frame, feature, Landmark}
+ // remove other: {Frame, Capture, Feature, Landmark}
FrameBasePtr frm_o = frame_other_ptr_.lock();
if (frm_o)
{
@@ -71,6 +71,14 @@ void ConstraintBase::remove()
frm_o->remove();
}
+ CaptureBasePtr cap_o = capture_other_ptr_.lock();
+ if (cap_o)
+ {
+ cap_o->getConstrainedByList().remove(shared_from_this());
+ if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ cap_o->remove();
+ }
+
FeatureBasePtr ftr_o = feature_other_ptr_.lock();
if (ftr_o)
{
diff --git a/src/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
index 91a2ba19e7f3aae58cbefca746a9dc4f9fcd8641..edc6cdd44cd4f49029db8f64baa9806263f35b17 100644
--- a/src/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -1,10 +1,10 @@
+#include "pinhole_tools.h"
#include "landmark_AHP.h"
#include "constraint_AHP.h"
#include "state_block.h"
#include "state_quaternion.h"
#include "sensor_camera.h"
#include "capture_image.h"
-#include "pinholeTools.h"
int main()
{
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 35407ef2b76185fc9b9466d35c21b7b3020efac6..07d272178f3f622745ff8def9848d2512523a426 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -187,10 +187,17 @@ int main(int argc, char** argv)
intrinsics_diff_drive->left_gain_ = 0.01;
intrinsics_diff_drive->right_gain_ = 0.01;
+ // Time and data variables
+ TimeStamp t;
+ Scalar stamp_secs(0);
+// Scalar period_secs(0.010); //100Hz
+ Scalar period_secs(0.020); //50Hz
+ Eigen::Vector2s data_; data_ << 0,0;
+
const auto scalar_max = std::numeric_limits<Scalar>::max();
ProcessorParamsBasePtr processor_params =
- std::make_shared<ProcessorParamsDiffDrive>(0.1, scalar_max, scalar_max, scalar_max);
+ std::make_shared<ProcessorParamsDiffDrive>(period_secs/2, scalar_max, scalar_max, scalar_max);
SensorBasePtr sensor_ptr =
wolf_problem_ptr_->installSensor("DIFF DRIVE", sensor_name, extrinsics, intrinsics);
@@ -203,13 +210,6 @@ int main(int argc, char** argv)
WOLF_INFO("Processor 'DIFF DRIVE' installed.");
- // Time and data variables
- TimeStamp t;
- Scalar stamp_secs(0);
-// Scalar period_secs(0.010); //100Hz
- Scalar period_secs(0.020); //50Hz
- Eigen::Vector2s data_; data_ << 0,0;
-
// Get initial wheel data
if (WHEEL_DATA)
readWheelData(data_file, data_);
diff --git a/src/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
index 9033c44975f1b9fa0b01797fd9f93a4053466a1d..fd22caf16d18077f38e0ca764c3ddd5985fe2ffb 100644
--- a/src/examples/test_projection_points.cpp
+++ b/src/examples/test_projection_points.cpp
@@ -7,7 +7,7 @@
#include <iostream>
//wolf includes
-#include "pinholeTools.h"
+#include "pinhole_tools.h"
int main(int argc, char** argv)
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 7429435a6c824eb5536b8b1834b08c8caeb9f4f0..8c9a029a9e76847f0253f2cf1d2a467ddec67e80 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -7,8 +7,8 @@
#include "wolf.h"
#include "frame_base.h"
+#include "pinhole_tools.h"
#include "sensor_camera.h"
-#include "pinholeTools.h"
#include "rotations.h"
#include "capture_image.h"
#include "landmark_AHP.h"
diff --git a/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
index 5b4f9f553fb4f0bb86a91a3947a4511270f59100..217a5adac8014149dae9acd6dc32b8069562aa9f 100644
--- a/src/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -5,7 +5,7 @@
* \author: jsola
*/
-#include "pinholeTools.h"
+#include "pinhole_tools.h"
#include "yaml/yaml_conversion.h"
#include "processor_image_feature.h"
#include "factory.h"
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 34f625fa16e5efc5f7a4feb1b64dd03e4a79b690..41a23ad30d9a13f00950f76e0281ec6e8060c72a 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -30,6 +30,8 @@ void FeatureBase::remove()
{
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
+
+ // remove from upstream
CaptureBasePtr C = capture_ptr_.lock();
if (C)
{
@@ -37,6 +39,8 @@ void FeatureBase::remove()
if (C->getFeatureList().empty())
C->remove(); // remove upstream
}
+
+ // remove downstream
while (!constraint_list_.empty())
{
constraint_list_.front()->remove(); // remove downstream
@@ -45,7 +49,6 @@ void FeatureBase::remove()
{
constrained_by_list_.front()->remove(); // remove constrained
}
-// std::cout << "Removed f" << id() << std::endl;
}
}
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 3318d01708db96e5bbd836cb92ecfb2437dda804..e5f42afb0981a19d3fd6c803c29fcbf565cb576e 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -69,12 +69,15 @@ void FrameBase::remove()
{
is_removing_ = true;
FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
+
+ // remove from upstream
TrajectoryBasePtr T = trajectory_ptr_.lock();
if (T)
{
T->getFrameList().remove(this_F); // remove from upstream
}
+ // remove downstream
while (!capture_list_.empty())
{
capture_list_.front()->remove(); // remove downstream
diff --git a/src/pinholeTools.h b/src/pinhole_tools.h
similarity index 99%
rename from src/pinholeTools.h
rename to src/pinhole_tools.h
index eb87340753e412d6b3598898d39ef8b62d45e2f9..bfb649787c5bb033dee630a0055338e7c97c3ab9 100644
--- a/src/pinholeTools.h
+++ b/src/pinhole_tools.h
@@ -2,7 +2,7 @@
#define PINHOLETOOLS_H
/**
- * \file pinholeTools.h
+ * \file pinhole_tools.h
*
* \date 06/04/2010
* \author jsola
diff --git a/src/problem.cpp b/src/problem.cpp
index e36dc43c0828fc688a964905a7169c42e2a3afe0..3989d2695e61c95b5b3b1068131f40461c9621ab 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -697,7 +697,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardwarePtr()->getSensorList().size()) + "S") : "") << endl;
if (depth >= 1)
{
- // Sensors
+ // Sensors =======================================================================================
for (auto S : getHardwarePtr()->getSensorList())
{
cout << " S" << S->id() << " " << S->getType();
@@ -742,7 +742,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
if (depth >= 2)
{
- // Processors
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (p->isMotion())
@@ -781,7 +781,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectoryPtr()->getFrameList().size()) + "F") : "") << endl;
if (depth >= 1)
{
- // Frames
+ // Frames =======================================================================================
for (auto F : getTrajectoryPtr()->getFrameList())
{
cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
@@ -809,7 +809,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
if (depth >= 2)
{
- // Captures
+ // Captures =======================================================================================
for (auto C : F->getCaptureList())
{
cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
@@ -822,6 +822,12 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
else
cout << " -> S-";
+ if (C->isMotion())
+ {
+ auto CM = std::static_pointer_cast<CaptureMotion>(C);
+ if (CM->getOriginFramePtr())
+ cout << " -> OF" << CM->getOriginFramePtr()->id();
+ }
cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
if (constr_by)
@@ -865,7 +871,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
if (depth >= 3)
{
- // Features
+ // Features =======================================================================================
for (auto f : C->getFeatureList())
{
cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
@@ -881,7 +887,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
<< " )" << endl;
if (depth >= 4)
{
- // Constraints
+ // Constraints =======================================================================================
for (auto c : f->getConstraintList())
{
cout << " c" << c->id() << " " << c->getType() << " -->";
@@ -907,7 +913,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMapPtr()->getLandmarkList().size()) + "L") : "") << endl;
if (depth >= 1)
{
- // Landmarks
+ // Landmarks =======================================================================================
for (auto L : getMapPtr()->getLandmarkList())
{
cout << " L" << L->id() << " " << L->getType();
@@ -967,6 +973,8 @@ bool Problem::check(int verbose_level)
}
// check pointer to Problem
is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+
+ // Sensors =======================================================================================
for (auto S : H->getSensorList())
{
if (verbose_level > 0)
@@ -989,6 +997,8 @@ bool Problem::check(int verbose_level)
// check problem and hardware pointers
is_consistent = is_consistent && (S->getProblem().get() == P_raw);
is_consistent = is_consistent && (S->getHardwarePtr() == H);
+
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (verbose_level > 0)
@@ -1012,6 +1022,8 @@ bool Problem::check(int verbose_level)
}
// check pointer to Problem
is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+
+ // Frames =======================================================================================
for (auto F : T->getFrameList())
{
if (verbose_level > 0)
@@ -1057,6 +1069,8 @@ bool Problem::check(int verbose_level)
}
}
}
+
+ // Captures =======================================================================================
for (auto C : F->getCaptureList())
{
if (verbose_level > 0)
@@ -1082,6 +1096,8 @@ bool Problem::check(int verbose_level)
// check problem and frame pointers
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
is_consistent = is_consistent && (C->getFramePtr() == F);
+
+ // Features =======================================================================================
for (auto f : C->getFeatureList())
{
if (verbose_level > 0)
@@ -1104,6 +1120,8 @@ bool Problem::check(int verbose_level)
// check constrained_by pointer to this feature
is_consistent = is_consistent && (cby->getFeatureOtherPtr() == f);
}
+
+ // Constraints =======================================================================================
for (auto c : f->getConstraintList())
{
if (verbose_level > 0)
@@ -1267,6 +1285,8 @@ bool Problem::check(int verbose_level)
cout << "M @ " << M.get() << endl;
// check pointer to Problem
is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+
+ // Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
if (verbose_level > 0)
diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp
index 14ab30164adb44619dc294c5a8e69533f07a0810..06b1522286268331335fdc8bbdedcf3eda87e4d8 100644
--- a/src/processor_IMU.cpp
+++ b/src/processor_IMU.cpp
@@ -4,7 +4,7 @@
namespace wolf {
ProcessorIMU::ProcessorIMU(const ProcessorParamsIMU& _params) :
- ProcessorMotion("IMU", 10, 10, 9, 6, 0.01, 6),
+ ProcessorMotion("IMU", 10, 10, 9, 6, 6, _params.time_tolerance),
max_time_span_ (_params.max_time_span ),
max_buff_length_(_params.max_buff_length ),
dist_traveled_ (_params.dist_traveled ),
diff --git a/src/processor_base.h b/src/processor_base.h
index 0061fb168e6712d907030680a193c85658f24f6e..7d7fe4226957be55b61cb601f57bc9cf08e7e416 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -20,6 +20,8 @@ namespace wolf {
/** \brief Key frame class pack
*
* To store a key_frame with an associated time tolerance.
+ *
+ * Used in keyframe callbacks as the minimal pack of information needed by the processor receiving the callback.
*/
class KFPack
{
diff --git a/src/processor_capture_holder.cpp b/src/processor_capture_holder.cpp
index cc34d1d2f42a271f57d818fbfb0ade5cc8088109..398f829aa397888878ed7a0ca4fee73080740315 100644
--- a/src/processor_capture_holder.cpp
+++ b/src/processor_capture_holder.cpp
@@ -90,7 +90,7 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
// {
// // go to the previous motion capture
// if (capture_ptr == last_ptr_)
-// capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+// capture_ptr = origin_ptr_;
// else if (capture_ptr->getOriginFramePtr() == nullptr)
// return nullptr;
// else
diff --git a/src/processor_image_landmark.cpp b/src/processor_image_landmark.cpp
index e9bd3cff8e0109f9e1152f27f46249cf919bcec2..7429f8b2f90b15decf94460422762532d2967bb9 100644
--- a/src/processor_image_landmark.cpp
+++ b/src/processor_image_landmark.cpp
@@ -1,11 +1,11 @@
#include "processor_image_landmark.h"
+#include "pinhole_tools.h"
#include "landmark_corner_2D.h"
#include "landmark_AHP.h"
#include "constraint_corner_2D.h"
#include "constraint_AHP.h"
#include "sensor_camera.h"
-#include "pinholeTools.h"
#include "trajectory_base.h"
#include "map_base.h"
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 731cb7f2f635dc405c0e6ff50cb1e06fc1f8d451..90ebde7321583660c47b0b86ec28239e902d9072 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -7,8 +7,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Scalar _time_tolerance,
- Size _calib_size) :
+ Size _calib_size,
+ Scalar _time_tolerance) :
ProcessorBase(_type, _time_tolerance),
processing_step_(RUNNING_WITHOUT_PACK),
x_size_(_state_size),
@@ -30,7 +30,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_size_, calib_size_)
{
- status_ = IDLE;
//
}
@@ -265,14 +264,25 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
capture_motion = last_ptr_;
else
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
- capture_motion = getCaptureMotionContainingTimeStamp(_ts);
+ capture_motion = findCaptureContainingTimeStamp(_ts);
- if (capture_motion)
+ if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
{
- // We found a CaptureMotion whose buffer contains the time stamp
- VectorXs state_0 = capture_motion->getOriginFramePtr()->getState();
- VectorXs delta = capture_motion->getDeltaCorrected(origin_ptr_->getCalibration(), _ts);
- Scalar dt = _ts - capture_motion->getBuffer().get().front().ts_;
+ // Get origin state and calibration
+ VectorXs state_0 = capture_motion->getOriginFramePtr()->getState();
+ CaptureBasePtr cap_orig = capture_motion->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+ VectorXs calib = cap_orig->getCalibration();
+
+ // Get delta and correct it with new bias
+ VectorXs calib_preint = capture_motion->getBuffer().getCalibrationPreint();
+ Motion motion = capture_motion->getBuffer().getMotion(_ts);
+
+ VectorXs delta_step = motion.jacobian_calib_ * (calib - calib_preint);
+ VectorXs delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
+
+ // Compose on top of origin state using the buffered time stamp, not the query time stamp
+ // TODO Interpolate the delta to produce a state at the query time stamp _ts
+ Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOriginPtr()->getTimeStamp();
statePlusDelta(state_0, delta, dt, _x);
}
else
@@ -284,34 +294,34 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
}
}
-CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
-{
- //std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl;
- CaptureMotionPtr capture_ptr = last_ptr_;
- while (capture_ptr != nullptr)
- {
- // capture containing motion previous than the ts found:
- if (capture_ptr->getBuffer().get().front().ts_ < _ts)
- return capture_ptr;
- else
- {
- // go to the previous motion capture
- if (capture_ptr == last_ptr_)
- capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
- else if (capture_ptr->getOriginFramePtr() == nullptr)
- return nullptr;
- else
- {
- CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
- if (capture_base_ptr == nullptr)
- return nullptr;
- else
- capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
- }
- }
- }
- return capture_ptr;
-}
+//CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
+//{
+// //std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl;
+// CaptureMotionPtr capture_ptr = last_ptr_;
+// while (capture_ptr != nullptr)
+// {
+// // capture containing motion previous than the ts found:
+// if (capture_ptr->getBuffer().get().front().ts_ < _ts)
+// return capture_ptr;
+// else
+// {
+// // go to the previous motion capture
+// if (capture_ptr == last_ptr_)
+// capture_ptr = origin_ptr_;
+// else if (capture_ptr->getOriginFramePtr() == nullptr)
+// return nullptr;
+// else
+// {
+// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+// if (capture_base_ptr == nullptr)
+// return nullptr;
+// else
+// capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
+// }
+// }
+// }
+// return capture_ptr;
+//}
FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
{
@@ -505,27 +515,28 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta
}
-CaptureMotionPtr ProcessorMotion::getCaptureMotionContainingTimeStamp(const TimeStamp& _ts)
+CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
// Note: since the buffer goes from a FK through the past until the previous KF, we need to:
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
- FrameBasePtr frame = nullptr;
- CaptureBasePtr capture = nullptr;
+ FrameBasePtr frame = nullptr;
+ CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_iter = getProblem()->getTrajectoryPtr()->getFrameList().rbegin();
- frame_iter != getProblem()->getTrajectoryPtr()->getFrameList().rend(); ++frame_iter)
+ for (auto frame_rev_iter = getProblem()->getTrajectoryPtr()->getFrameList().rbegin();
+ frame_rev_iter != getProblem()->getTrajectoryPtr()->getFrameList().rend();
+ ++frame_rev_iter)
{
- frame = *frame_iter;
+ frame = *frame_rev_iter;
capture = frame->getCaptureOf(getSensorPtr());
if (capture != nullptr)
{
// We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
if (_ts >= capture_motion->getBuffer().get().front().ts_)
- // Found time stamp satisfying rule 3 above !!
+ // Found time stamp satisfying rule 3 above !! ==> break for loop
break;
}
}
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 93d1af04a43ee77e02661331d1beb8031e2ed49a..2f88d71a9c936c77eafb86668281b241ddb4e4d1 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -110,13 +110,6 @@ class ProcessorMotion : public ProcessorBase
protected:
ProcessingStep processing_step_; ///< State machine controlling the processing step
- private:
- enum
- {
- IDLE,
- RUNNING
- } status_;
-
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
@@ -124,8 +117,8 @@ class ProcessorMotion : public ProcessorBase
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Scalar _time_tolerance = 0.1,
- Size _calib_size = 0);
+ Size _calib_size = 0,
+ Scalar _time_tolerance = 0.1);
virtual ~ProcessorMotion();
// Instructions to the processor:
@@ -404,10 +397,9 @@ class ProcessorMotion : public ProcessorBase
virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
Motion motionZero(const TimeStamp& _ts);
- CaptureMotionPtr getCaptureMotionContainingTimeStamp(const TimeStamp& _ts);
public:
- virtual CaptureBasePtr getOriginPtr();
+ virtual CaptureMotionPtr getOriginPtr();
virtual CaptureMotionPtr getLastPtr();
virtual CaptureMotionPtr getIncomingPtr();
@@ -419,7 +411,7 @@ class ProcessorMotion : public ProcessorBase
Size delta_size_; ///< the size of the deltas
Size delta_cov_size_; ///< the size of the delta covariances matrix
Size calib_size_; ///< size of the extra parameters (TBD in derived classes)
- CaptureBasePtr origin_ptr_;
+ CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
@@ -541,7 +533,7 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
);
}
-inline CaptureBasePtr ProcessorMotion::getOriginPtr()
+inline CaptureMotionPtr ProcessorMotion::getOriginPtr()
{
return origin_ptr_;
}
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index 8cd3b12a08956f50c80610e3aa04acc9bf41923f..38c37f5c15c42cb38fe978e835b949d910c7aaaf 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -3,11 +3,11 @@ namespace wolf
{
ProcessorOdom2D::ProcessorOdom2D(const ProcessorParamsOdom2D& _params) :
- ProcessorMotion("ODOM 2D", 3, 3, 3, 2),
- dist_traveled_th_(_params.dist_traveled_th_),
- theta_traveled_th_(_params.theta_traveled_th_),
- cov_det_th_(_params.cov_det_th_),
- elapsed_time_th_(_params.elapsed_time_th_)
+ ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params.time_tolerance),
+ dist_traveled_th_(_params.dist_traveled_th_),
+ theta_traveled_th_(_params.theta_traveled_th_),
+ cov_det_th_(_params.cov_det_th_),
+ elapsed_time_th_(_params.elapsed_time_th_)
{
unmeasured_perturbation_cov_ = _params.unmeasured_perturbation_std_ * _params.unmeasured_perturbation_std_ * Matrix3s::Identity();
}
diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index 38c337fd8e19a0c26bf590ab85b98ab263b5da4b..b5e619163571bdf11ee888d8631ad5ef62826b18 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3D::ProcessorOdom3D(const ProcessorParamsOdom3D& _params, SensorOdom3DPtr _sensor_ptr) :
- ProcessorMotion("ODOM 3D", 7, 7, 6, 6),
+ ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params.time_tolerance ),
max_time_span_ ( _params.max_time_span ),
max_buff_length_( _params.max_buff_length ),
dist_traveled_ ( _params.dist_traveled ),
diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp
index 535cd5c178b0f184d23cfcc95d3689683a8464cb..8031040ce0b184d9c300143897925141140e5e3a 100644
--- a/src/processors/processor_diff_drive.cpp
+++ b/src/processors/processor_diff_drive.cpp
@@ -13,7 +13,7 @@ namespace wolf
{
ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrive ¶ms) :
- ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 0.15, 3),
+ ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 3, 0.15),
unmeasured_perturbation_cov_(Matrix3s::Identity()*
params.unmeasured_perturbation_std_*
params.unmeasured_perturbation_std_),
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 83578afef1d86452eeac5473b397ae87403617b4..942fff7ee26007e0b9ad69826107227f3dac3d10 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -218,18 +218,21 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _noise_cov;
}
-CaptureBasePtr SensorBase::lastCapture(void)
+CaptureBasePtr SensorBase::lastKeyCapture(void)
{
- // we search for the most recent Capture of this sensor
+ // we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
FrameBaseRevIter frame_rev_it = frame_list.rbegin();
while (frame_rev_it != frame_list.rend())
{
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
+ if ((*frame_rev_it)->isKey())
+ {
+ capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ break;
+ }
frame_rev_it++;
}
return capture;
@@ -340,7 +343,7 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
- CaptureBasePtr cap = lastCapture();
+ CaptureBasePtr cap = lastKeyCapture();
if (cap)
return cap->getStateBlockPtr(_i);
else
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 8c6b6c168d96023e12f80f819bd467aa1aa7217a..3a2de9957d572026fb637f0b1d19932974171686 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -102,7 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
ProcessorBaseList& getProcessorList();
- CaptureBasePtr lastCapture(void);
+ CaptureBasePtr lastKeyCapture(void);
CaptureBasePtr lastCapture(const TimeStamp& _ts);
bool process(const CaptureBasePtr capture_ptr);
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index 79e6ae700af7062fc2926027cae76b5192877813..36e6c9ad53f3d19c37560eeb9a756398acbab736 100644
--- a/src/sensor_camera.cpp
+++ b/src/sensor_camera.cpp
@@ -1,7 +1,8 @@
#include "sensor_camera.h"
+
+#include "pinhole_tools.h"
#include "state_block.h"
#include "state_quaternion.h"
-#include "pinholeTools.h"
namespace wolf
{
diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp
index 52e4177c7c6896c7da6b421194ffbc8278eb8e00..0931b700ce658b486823fa930e3f1c454f87be5e 100644
--- a/src/sensors/sensor_diff_drive.cpp
+++ b/src/sensors/sensor_diff_drive.cpp
@@ -103,24 +103,22 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
// problem->installSensor() return a SensorBasePtr.
//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
//{
-// std::shared_ptr<CaptureMotion> capture_ptr = std::static_pointer_cast<CaptureMotion>(_capture_ptr);
-
// if (intrinsics_.data_is_position_)
// {
-// Eigen::Vector2s data = capture_ptr->getData();
+// Eigen::Vector2s data = _capture_ptr->getData();
// // dt is set to one as we are dealing with wheel position
// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
// intrinsics_.separation_, 1);
-// capture_ptr->setData(data);
+// _capture_ptr->setData(data);
// Eigen::Matrix2s data_cov;
// data_cov << 0.00001, 0, 0, 0.00001; // Todo
// computeDataCov(data, data_cov);
-// capture_ptr->setDataCovariance(data_cov);
+// _capture_ptr->setDataCovariance(data_cov);
// }
// /// @todo tofix
diff --git a/src/test/gtest_constraint_IMU.cpp b/src/test/gtest_constraint_IMU.cpp
index a507efbc0052ddf42863077c639f3448d787cc54..35c5155373f43765b6ad2fce8d95ab1ce19e826d 100644
--- a/src/test/gtest_constraint_IMU.cpp
+++ b/src/test/gtest_constraint_IMU.cpp
@@ -979,9 +979,9 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
sensor_odo->process(capture_odo);
-// WOLF_TRACE("Jac calib: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getJacobianCalib().row(0));
+// WOLF_TRACE("Jac calib: ", processor_imu->getOriginPtr()->getJacobianCalib().row(0));
// WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getCalibrationPreint().transpose());
+// WOLF_TRACE("orig calib preint: ", processor_imu->getOriginPtr()->getCalibrationPreint().transpose());
//prepare next odometry measurement
quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
diff --git a/src/test/gtest_pinhole.cpp b/src/test/gtest_pinhole.cpp
index 68b951c00abed88eec483400b3a2c5e0653c5507..b46e682ab406df2db2f84a23c72f9669e777df8c 100644
--- a/src/test/gtest_pinhole.cpp
+++ b/src/test/gtest_pinhole.cpp
@@ -5,9 +5,9 @@
* Author: jsola
*/
+#include "pinhole_tools.h"
#include "utils_gtest.h"
-#include "../pinholeTools.h"
using namespace Eigen;
using namespace wolf;
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index cf0a563391970fc136001cfc3ff28b5d316db47c..38552b3334d5e0798f29aed417355e11d4ce94f7 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -438,7 +438,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
problem->setPrior(x0, P0, t, 0.01);
- std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+ problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
@@ -494,7 +494,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6s bias; bias << abx,aby,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
- std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+ problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
diff --git a/src/three_D_tools.h b/src/three_D_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..28a15f29757116ffc1c9bc10f9507df58f9a5452
--- /dev/null
+++ b/src/three_D_tools.h
@@ -0,0 +1,375 @@
+/*
+ * three_D_tools.h
+ *
+ * Created on: Mar 15, 2018
+ * Author: jsola
+ */
+
+#ifndef THREE_D_TOOLS_H_
+#define THREE_D_TOOLS_H_
+
+
+#include "wolf.h"
+#include "rotations.h"
+
+/*
+ * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
+ *
+ * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
+ * Delta = [Dp, Dq]
+ * with
+ * Dp : position delta
+ * Dq : quaternion delta
+ *
+ * The functions are listed below:
+ *
+ * - compose: Dc = D1 (+) D2
+ * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
+ * - inverse: so that D (+) D.inv = D.inv (+) D = I
+ * - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
+ * - lift: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 (+) retract(d)
+ * - diff: d = lift( D2 (-) D1 )
+ */
+
+
+
+namespace wolf
+{
+namespace three_D {
+using namespace Eigen;
+
+template<typename D1, typename D2>
+inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
+{
+ p = MatrixBase<D1>::Zero(3,1);
+ q = QuaternionBase<D2>::Identity();
+}
+
+template<typename D1, typename D2>
+inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
+{
+ typedef typename D1::Scalar T1;
+ typedef typename D2::Scalar T2;
+ p << T1(0), T1(0), T1(0);
+ q << T2(0), T2(0), T2(0), T2(1);
+}
+
+template<typename T = wolf::Scalar>
+inline Matrix<T, 7, 1> identity()
+{
+ Matrix<T, 7, 1> ret;
+ ret<< T(0), T(0), T(0),
+ T(0), T(0), T(0), T(1);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D4, typename D5>
+inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
+ MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
+{
+ MatrixSizeCheck<3, 1>::check(dp);
+ MatrixSizeCheck<3, 1>::check(idp);
+
+ idp = - dq.conjugate() * dp ;
+ idq = dq.conjugate() ;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+ MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<7, 1>::check(d);
+ MatrixSizeCheck<7, 1>::check(id);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
+ Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
+ Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
+
+ inverse(dp, dq, idp, idq);
+}
+
+
+template<typename D>
+inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
+{
+ Matrix<typename D::Scalar, 7, 1> id;
+ inverse(d, id);
+ return id;
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+ MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
+{
+ MatrixSizeCheck<3, 1>::check(dp1);
+ MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(sum_p);
+
+ sum_p = dp1 + dq1*dp2;
+ sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& sum)
+{
+ MatrixSizeCheck<7, 1>::check(d1);
+ MatrixSizeCheck<7, 1>::check(d2);
+ MatrixSizeCheck<7, 1>::check(sum);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
+ Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
+
+ compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2 )
+{
+ Matrix<typename D1::Scalar, 7, 1> ret;
+ compose(d1, d2, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
+{
+ MatrixSizeCheck<7, 1>::check(d1);
+ MatrixSizeCheck<7, 1>::check(d2);
+ MatrixSizeCheck<7, 1>::check(sum);
+ MatrixSizeCheck< 6, 6>::check(J_sum_d1);
+ MatrixSizeCheck< 6, 6>::check(J_sum_d2);
+
+ // Some useful temporaries
+ Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
+ Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
+
+ // Jac wrt first delta
+ J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
+ J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
+ J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
+
+ // Jac wrt second delta
+ J_sum_d2.setIdentity(); //
+ J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
+ // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
+
+ // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
+ compose(d1, d2, sum);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+ MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
+{
+ MatrixSizeCheck<3, 1>::check(dp1);
+ MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(dp12);
+
+ dp12 = dq1.conjugate() * ( dp2 - dp1 );
+ dq12 = dq1.conjugate() * dq2;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d2_minus_d1)
+{
+ MatrixSizeCheck<7, 1>::check(d1);
+ MatrixSizeCheck<7, 1>::check(d2);
+ MatrixSizeCheck<7, 1>::check(d2_minus_d1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
+ Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) );
+
+ between(dp1, dq1, dp2, dq2, dp12, dq12);
+}
+
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2 )
+{
+ Matrix<typename D1::Scalar, 7, 1> d12;
+ between(d1, d2, d12);
+ return d12;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
+{
+ MatrixSizeCheck<7, 1>::check(delta_in);
+
+ Matrix<typename Derived::Scalar, 6, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
+ Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
+
+ dp_ret = dp_in;
+ do_ret = log_q(dq_in);
+
+ return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
+{
+ MatrixSizeCheck<6, 1>::check(d_in);
+
+ Matrix<typename Derived::Scalar, 7, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
+ Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
+
+ dp = dp_in;
+ dq = exp_q(do_in);
+
+ return ret;
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
+ MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
+{
+ plus_p = dp1 + dp2;
+ plus_q = dq1 * exp_q(do2);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d_plus)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
+ Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) );
+
+ plus(dp1, dq1, dp2, do2, dp_p, dq_p);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 7, 1> d_plus;
+ plus(d1, d2, d_plus);
+ return d_plus;
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+ MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
+{
+ diff_p = dp2 - dp1;
+ diff_o = log_q(dq1.conjugate() * dq2);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+ MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
+ MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
+{
+ diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+
+ J_do_dq1 = - jac_SO3_left_inv(diff_o);
+ J_do_dq2 = jac_SO3_right_inv(diff_o);
+}
+
+
+template<typename D1, typename D2, typename D3>
+inline void diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& err)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
+
+ diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& dif,
+ MatrixBase<D4>& J_diff_d1,
+ MatrixBase<D5>& J_diff_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
+
+ Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+
+ diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+
+ /* d = diff(d1, d2) is
+ * dp = dp2 - dp1
+ * do = Log(dq1.conj * dq2)
+ * dv = dv2 - dv1
+ *
+ * With trivial Jacobians for dp and dv, and:
+ * J_do_dq1 = - J_l_inv(theta)
+ * J_do_dq2 = J_r_inv(theta)
+ */
+
+ J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
+ J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+
+ J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity
+ J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 1> diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 6, 1> ret;
+ diff(d1, d2, ret);
+ return ret;
+}
+
+
+} // namespace three_d
+} // namespace wolf
+
+
+#endif /* THREE_D_TOOLS_H_ */