diff --git a/src/ceres_wrapper/create_auto_diff_cost_function.cpp b/src/ceres_wrapper/create_auto_diff_cost_function.cpp
index 3d2809b2d028dfee319fdffdba1e17f66e1c8dbc..c0e3edddd5fcc215e18b628e3e05854281b3dcfa 100644
--- a/src/ceres_wrapper/create_auto_diff_cost_function.cpp
+++ b/src/ceres_wrapper/create_auto_diff_cost_function.cpp
@@ -14,6 +14,7 @@
#include "../constraint_gps_pseudorange_3D.h"
#include "../constraint_gps_pseudorange_2D.h"
#include "../constraint_odom_2D.h"
+#include "../constraint_odom_3D.h"
#include "../constraint_corner_2D.h"
#include "../constraint_point_2D.h"
#include "../constraint_point_to_line_2D.h"
@@ -49,6 +50,12 @@ ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool
else
return createAutoDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
+ case CTR_ODOM_3D:
+ if (_use_wolf_autodiff)
+ return createAutoDiffCostFunctionWrapper<ConstraintOdom3D>(_ctr_ptr);
+ else
+ return createAutoDiffCostFunctionCeres<ConstraintOdom3D>(_ctr_ptr);
+
case CTR_CORNER_2D:
if (_use_wolf_autodiff)
return createAutoDiffCostFunctionWrapper<ConstraintCorner2D>(_ctr_ptr);
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index 1d58ea77ddc9a8ae8f4d134373c388c5b946337a..a513483263888cdad14995d8bdf79e2d04417019 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -29,8 +29,33 @@ class ConstraintOdom3D : public ConstraintSparse<6,3,4,3,4>
const T* const _q2,
T* _residuals) const;
+ private:
+ template<typename T>
+ void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
+
+
};
+template<typename T>
+inline void ConstraintOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
+{
+ std::cout << "Jet residual = " << std::endl;
+ std::cout << r(0) << std::endl;
+ std::cout << r(1) << std::endl;
+ std::cout << r(2) << std::endl;
+ std::cout << r(3) << std::endl;
+ std::cout << r(4) << std::endl;
+ std::cout << r(5) << std::endl;
+}
+
+template<>
+inline void ConstraintOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) const
+{
+ std::cout << "Scalar residual = " << std::endl;
+ std::cout << r.transpose() << std::endl;
+}
+
+
inline ConstraintOdom3D::ConstraintOdom3D(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, bool _apply_loss_function,
ConstraintStatus _status) :
ConstraintSparse<6, 3, 4, 3, 4>(CTR_ODOM_3D, _frame_ptr, _apply_loss_function, _status,
@@ -96,14 +121,18 @@ inline bool wolf::ConstraintOdom3D::operator ()(const T* const _p1, const T* con
Eigen::Quaternion<T> dq = q1.conjugate() * q2;
// measured motion increment, dp_m, dq_m
- Eigen::Map<Eigen::Matrix<T,3,1>> dp_m(getMeasurement().data());
- Eigen::Quaternion<T> dq_m = v2q(getMeasurement().tail<3>());
+ Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
+ Eigen::Quaternion<T> dq_m = v2q(getMeasurement().tail<3>()).cast<T>();
// residual
// residuals.head<3>() = dq.conjugate() * (dp_m - dp); // see note below
residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
residuals.tail(3) = q2v(dq.conjugate() * dq_m);
+ Eigen::Matrix<T,6,1> r = residuals;
+
+// printRes(r);
+
return true;
}
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index 13e77a26872d021672895c2d492fcfb357c63a85..b63732b21931019a31e5d82f4c64aabfc401d4c2 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -23,7 +23,6 @@
#include "../sensor_odom_2D.h"
#include "../sensor_gps_fix.h"
#include "../capture_fix.h"
-#include "../capture_odom_2D.h"
#include "../ceres_wrapper/ceres_manager.h"
// laserscanutils
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index eecb272ae3d9497be941901bef632d9a140f7cf7..780af80435c7d3ed98b99ae83bd18bcabfeacfb8 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -23,7 +23,6 @@
#include "sensor_odom_2D.h"
#include "sensor_gps_fix.h"
#include "capture_fix.h"
-#include "capture_odom_2D.h"
#include "ceres_wrapper/ceres_manager.h"
// laserscanutils
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index fa0e6990f89adf7db81bea0619906fe85b7c0cd9..b42b4da08805331c5324004a7aeb3bcfabfd1261 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -23,7 +23,6 @@
#include "sensor_odom_2D.h"
#include "sensor_gps_fix.h"
#include "capture_fix.h"
-#include "capture_odom_2D.h"
#include "ceres_wrapper/ceres_manager.h"
// laserscanutils
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index d0c4808505ee8bf499caaa2891fc4dc8c9d7af8a..da3511e90a046a781fb9ca218cafdccec36ddfce 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -10,6 +10,11 @@
#include "sensor_odom_2D.h"
#include "processor_odom_3D.h"
#include "capture_imu.h"
+#include "map_base.h"
+#include "landmark_base.h"
+#include "ceres_wrapper/ceres_manager.h"
+
+#include <cstdlib>
using namespace wolf;
using std::cout;
@@ -20,11 +25,75 @@ using Eigen::Vector7s;
using Eigen::Quaternions;
using Eigen::VectorXs;
-int main ()
+void print(Problem* P)
+{
+ cout << "P: wolf tree status:" << endl;
+ cout << "H" << endl;
+ for (auto S : *(P->getHardwarePtr()->getSensorListPtr() ) )
+ {
+ cout << " S" << S->id() << endl;
+ for (auto p : *(S->getProcessorListPtr() ) )
+ {
+ cout << " p" << p->id() << endl;
+ }
+ }
+ cout << "T" << endl;
+ for (auto F : *(P->getTrajectoryPtr()->getFrameListPtr() ) )
+ {
+ cout << (F->isKey() ? " KF" : " F") << F->id() << (F->isFixed() ? ", fixed" : ", estim") << ", ts=" << F->getTimeStamp().get();
+ cout << ",\t x = ( " << F->getState().transpose() << ")" << endl;
+ for (auto C : *(F->getCaptureListPtr() ) )
+ {
+ cout << " C" << C->id() << endl;
+ for (auto f : *(C->getFeatureListPtr() ) )
+ {
+ cout << " f" << f->id() << ", \t m = ( " << std::setprecision(2) << f->getMeasurement().transpose() << ")" << endl;
+ for (auto c : *(f->getConstraintListPtr() ) )
+ {
+ cout << " c" << c->id();
+ switch (c->getCategory())
+ {
+ case CTR_ABSOLUTE:
+ cout << " --> A" << endl;
+ break;
+ case CTR_FRAME:
+ cout << " --> F" << c->getFrameOtherPtr()->id() << endl;
+ break;
+ case CTR_FEATURE:
+ cout << " --> f" << c->getFeatureOtherPtr()->id() << endl;
+ break;
+ case CTR_LANDMARK:
+ cout << " --> L" << c->getLandmarkOtherPtr()->id() << endl;
+ break;
+ }
+ }
+ }
+ }
+ }
+ cout << "M" << endl;
+ for (auto L : *(P->getMapPtr()->getLandmarkListPtr() ) )
+ {
+ cout << " L" << L->id() << endl;
+ }
+}
+
+
+int main (int argc, char** argv)
{
+ cout << "\n========= Test ProcessorOdom3D ===========" << endl;
+
+ TimeStamp tf;
+ if (argc == 1)
+ tf = 0.3;
+ else
+ {
+
+ tf.set(strtod(argv[1],nullptr));
+ }
+ cout << "Final timestamp tf = " << tf.get() << " s" << endl;
Problem* problem = new Problem(FRM_PO_3D);
- CeresManager* ceres_manager = new CeresManager(problem);
+ CeresManager ceres_manager(problem);
SensorBase* sen = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(),"");
problem->installProcessor("ODOM 3D", "odometry integrator", "odom", "");
@@ -35,21 +104,32 @@ int main ()
Vector6s data((Vector6s() << d_pos , d_theta).finished()); // will integrate this data repeatedly
- Scalar dt = 0.01;
+ Scalar dt = 0.1;
- for (TimeStamp t = 0; t < 5 - Constants::EPS; t += dt)
+ cout << "t: " << 0 << " \t\t\t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
+ print(problem);
+ cout << "--------------------------------------------------------------" << endl;
+
+ for (TimeStamp t = dt; t < tf - Constants::EPS; t += dt)
{
CaptureMotion* cap_odo = new CaptureMotion(t, sen, data);
cap_odo->process();
- cout << "t: " << t.get() << " x: " << problem->getCurrentState().transpose() << endl;
+ cout << "t: " << t.get() << " \t\t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
+ print(problem);
+ cout << "--------------------------------------------------------------" << endl;
+
+ ceres::Solver::Summary summary = ceres_manager.solve();
+
+// ceres_manager.computeCovariances(ALL);
+
+// cout << summary.BriefReport() << endl;
- ceres::Solver::Summary summary = ceres_manager->solve();
- cout << summary << endl;
}
- delete problem;
+ delete problem; // XXX Why is this throwing segfault?
+
return 0;
}
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
index c60d2f11c6d6e6435c9d4a057333ed6ef33717e9..0b9c3fef9c9af7feb465a2f7a47d17e16e28990e 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -16,7 +16,7 @@ HardwareBase::~HardwareBase()
is_deleting_ = true;
while (!sensor_list_.empty())
{
- delete sensor_list_.front();
+ sensor_list_.front()->destruct();
sensor_list_.pop_front();
}
//std::cout << "deleting HardwareBase " << nodeId() << std::endl;
@@ -27,10 +27,8 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
//std::cout << "adding sensor..." << std::endl;
sensor_list_.push_back(_sensor_ptr);
_sensor_ptr->setProblem(getProblem());
-// addDownNode(_sensor_ptr);
//std::cout << "added!" << std::endl;
-
_sensor_ptr->registerNewStateBlocks();
return _sensor_ptr;
@@ -41,7 +39,6 @@ void HardwareBase::removeSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.remove(_sensor_ptr);
delete _sensor_ptr;
-// removeDownNode(_sensor_ptr->nodeId());
}
} // namespace wolf
diff --git a/src/map_base.cpp b/src/map_base.cpp
index 3192c5a25de540fcbebf7394edd6363ffd482d54..5e77d7ea9d3b5f2b7c4fefd0dd674e67086c0b42 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -30,7 +30,7 @@ MapBase::~MapBase()
is_deleting_ = true;
while (!landmark_list_.empty())
{
- delete landmark_list_.front();
+ landmark_list_.front()->destruct();
landmark_list_.pop_front();
}
diff --git a/src/problem.h b/src/problem.h
index 1a7a8fb14d03e286740af6afe7b26f5ea9a69eb2..890372775594ac5ac603a5623a27715ac9532f95 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -268,11 +268,6 @@ class Problem
};
-inline ProcessorMotion* Problem::getProcessorMotionPtr()
-{
- return processor_motion_ptr_;
-}
-
} // namespace wolf
// IMPLEMENTATION
@@ -280,6 +275,11 @@ inline ProcessorMotion* Problem::getProcessorMotionPtr()
namespace wolf
{
+inline ProcessorMotion* Problem::getProcessorMotionPtr()
+{
+ return processor_motion_ptr_;
+}
+
inline std::list<StateBlockNotification>& Problem::getStateBlockNotificationList()
{
return state_block_notification_list_;
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 412cd5484341013252d93aef3dcbbb133b9f4880..f9c8e3a073674aac17cc1aa0e123344f7faa284f 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -459,6 +459,9 @@ inline void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
deltaZero(),
Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_),
Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_)}));
+ delta_integrated_ = deltaZero();
+ delta_integrated_cov_.setZero();
+
// reset derived things
resetDerived();
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index b40af6e1fb3c953b9edd7b51e6c04ec86155d86a..d4c0307f7edea3b30bc452b59133f9523241acdd 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -66,8 +66,8 @@ class ProcessorOdom3D : public ProcessorMotion
Motion& _motion,
TimeStamp& _ts);
bool voteForKeyFrame();
-
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin);
+ ConstraintBasePtr createConstraint(FeatureBasePtr _feature_motion,
+ FrameBasePtr _frame_origin);
private:
Eigen::Map<const Eigen::Vector3s> p1_, p2_;
@@ -78,10 +78,13 @@ class ProcessorOdom3D : public ProcessorMotion
// Factory method
public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
+ static ProcessorBasePtr create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params);
};
-inline void ProcessorOdom3D::data2delta(const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, const Scalar _dt)
+inline void ProcessorOdom3D::data2delta(const Eigen::VectorXs& _data,
+ const Eigen::MatrixXs& _data_cov,
+ const Scalar _dt)
{
delta_.head<3>() = _data.head<3>();
new (&q_out_) Eigen::Map<Eigen::Quaternions>(delta_.data() + 3);
@@ -104,7 +107,10 @@ inline void ProcessorOdom3D::data2delta(const Eigen::VectorXs& _data, const Eige
delta_cov_ = _data_cov;
}
-inline void ProcessorOdom3D::xPlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt, Eigen::VectorXs& _x_plus_delta)
+inline void ProcessorOdom3D::xPlusDelta(const Eigen::VectorXs& _x,
+ const Eigen::VectorXs& _delta,
+ const Scalar _Dt,
+ Eigen::VectorXs& _x_plus_delta)
{
assert(_x.size() == x_size_ && "Wrong _x vector size");
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
@@ -117,7 +123,10 @@ inline void ProcessorOdom3D::xPlusDelta(const Eigen::VectorXs& _x, const Eigen::
}
-inline void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2, Eigen::VectorXs& _delta1_plus_delta2)
+inline void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1,
+ const Eigen::VectorXs& _delta2,
+ const Scalar _Dt2,
+ Eigen::VectorXs& _delta1_plus_delta2)
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
@@ -128,9 +137,11 @@ inline void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, cons
q_out_ = q1_ * q2_;
}
-inline void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2,
+inline void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1,
+ const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
+ Eigen::VectorXs& _delta1_plus_delta2,
+ Eigen::MatrixXs& _jacobian1,
Eigen::MatrixXs& _jacobian2)
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
@@ -173,7 +184,9 @@ inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
return (Eigen::VectorXs(7) << 0,0,0, 0,0,0,1).finished(); // p, q
}
-inline Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _motion, TimeStamp& _ts)
+inline Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
+ Motion& _motion,
+ TimeStamp& _ts)
{
Motion tmp(_motion_ref);
tmp.ts_ = _ts;
@@ -187,12 +200,15 @@ inline bool ProcessorOdom3D::voteForKeyFrame()
return true;
}
-inline ConstraintBasePtr ProcessorOdom3D::createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin)
+inline ConstraintBasePtr ProcessorOdom3D::createConstraint(FeatureBasePtr _feature_motion,
+ FrameBasePtr _frame_origin)
{
return new ConstraintOdom3D(_feature_motion, _frame_origin);
}
-inline void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out)
+inline void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1,
+ const Eigen::VectorXs& _x2,
+ Eigen::VectorXs& _x_out)
{
new (&p1_) Eigen::Map<const Eigen::Vector3s>(_x1.data());
new (&q1_) Eigen::Map<const Eigen::Quaternions>(_x1.data() + 3);
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index c0546d299a409b8b4b536255ea514958ce6d7935..abe926757af3acb788cb16673a3f7eeeaff55636 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -69,7 +69,7 @@ SensorBase::~SensorBase()
while (!processor_list_.empty())
{
- delete processor_list_.front();
+ processor_list_.front()->destruct();
processor_list_.pop_front();
}
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 18720c7d8b48dcb905fe67a7d51945a59b7e84b3..fcd0a436e9beb65e9cedcd4e55cfcec5b1a14bd2 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -16,7 +16,7 @@ TrajectoryBase::~TrajectoryBase()
is_deleting_ = true;
while (!frame_list_.empty())
{
- delete frame_list_.front();
+ frame_list_.front()->destruct();
frame_list_.pop_front();
}
//std::cout << "deleting TrajectoryBase " << nodeId() << std::endl;
@@ -66,7 +66,7 @@ void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
{
for (auto frm_rit = getFrameListPtr()->rbegin(); frm_rit != getFrameListPtr()->rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() < _frame_ptr->getTimeStamp())
+ if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
return frm_rit.base();
return getFrameListPtr()->begin();
}
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index 4e43d98a4c5df232146f148801c8c6109c13af42..50d9e19f41e33cb9d2050d9a8af9b4f08ad141e6 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -55,7 +55,7 @@ class TrajectoryBase : public NodeBase
FrameBasePtr getLastKeyFramePtr();
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
- void sortFrame(FrameBasePtr _frame_iter);
+ void sortFrame(FrameBasePtr _frm_ptr);
void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);