diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index c07a5179a4376608acbd17014f48e1f27c282569..61dbc8131c098542b8ce0e759873f3854c9316f6 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -234,7 +234,7 @@ void CaptureLaser2D::establishConstraintsMHTree()
// Global transformation TODO: Change by a function
Eigen::Vector2s t_robot = getFramePtr()->getPPtr()->getVector();
Eigen::Matrix2s R_robot = ((StateOrientation*) (getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2,2>();
- WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());
+ WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());//TODO: it only works for orientation theta
// Sensor transformation
Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
@@ -269,7 +269,7 @@ void CaptureLaser2D::establishConstraintsMHTree()
tree.solve(ft_lk_pairs,associated_mask);
//print tree & score table
-// std::cout << "-------------" << std::endl;
+// std::cout << "-------------" << std::endl;
// tree.printTree();
// tree.printScoreTable();
@@ -358,6 +358,7 @@ WolfScalar CaptureLaser2D::computeMahalanobisDistance(const FeatureBase* _featur
Eigen::Map<Eigen::MatrixXs> frame_p_frame_o_cov(&Sigma(0,frame_p_size),frame_p_size,frame_o_size);
Eigen::Map<Eigen::MatrixXs> frame_p_landmark_p_cov(&Sigma(0,frame_p_size+frame_o_size),frame_p_size,landmark_p_size);
Eigen::Map<Eigen::MatrixXs> frame_p_landmark_o_cov(&Sigma(0,frame_p_size+frame_o_size+landmark_p_size),frame_p_size,landmark_o_size);
+
getTop()->getCovarianceBlock(frame_ptr->getPPtr(), frame_ptr->getPPtr(), frame_p_frame_p_cov);
getTop()->getCovarianceBlock(frame_ptr->getPPtr(), frame_ptr->getOPtr(), frame_p_frame_o_cov);
getTop()->getCovarianceBlock(frame_ptr->getPPtr(), _landmark_ptr->getPPtr(), frame_p_landmark_p_cov);
@@ -366,12 +367,14 @@ WolfScalar CaptureLaser2D::computeMahalanobisDistance(const FeatureBase* _featur
Eigen::Map<Eigen::MatrixXs> frame_o_frame_o_cov(&Sigma(frame_p_size,frame_p_size),frame_o_size,frame_o_size);
Eigen::Map<Eigen::MatrixXs> frame_o_landmark_p_cov(&Sigma(frame_p_size,frame_p_size+frame_o_size),frame_o_size,landmark_p_size);
Eigen::Map<Eigen::MatrixXs> frame_o_landmark_o_cov(&Sigma(frame_p_size,frame_p_size+frame_o_size+landmark_p_size),frame_o_size,landmark_o_size);
+
getTop()->getCovarianceBlock(frame_ptr->getOPtr(), frame_ptr->getOPtr(), frame_o_frame_o_cov);
getTop()->getCovarianceBlock(frame_ptr->getOPtr(), _landmark_ptr->getPPtr(), frame_o_landmark_p_cov);
getTop()->getCovarianceBlock(frame_ptr->getOPtr(), _landmark_ptr->getOPtr(), frame_o_landmark_o_cov);
Eigen::Map<Eigen::MatrixXs> landmark_p_landmark_p_cov(&Sigma(frame_p_size+frame_o_size,frame_p_size+frame_o_size),landmark_p_size,landmark_p_size);
Eigen::Map<Eigen::MatrixXs> landmark_p_landmark_o_cov(&Sigma(frame_p_size+frame_o_size,frame_p_size+frame_o_size+landmark_p_size),landmark_p_size,landmark_o_size);
+
getTop()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getPPtr(), landmark_p_landmark_p_cov);
getTop()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getOPtr(), landmark_p_landmark_o_cov);
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index 8da893452b6000ba932ef30dfc3720b2718ef4f9..8bd303448c29754573826993da4c1650ded01c9e 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -10,9 +10,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, cons
CaptureRelative(_ts, _sensor_ptr, _data)
{
data_covariance_ = Eigen::Matrix3s::Zero();
- data_covariance_(0, 0) = (_data(0) == 0 ? 1e-6 : fabs(_data(0))) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor();
- data_covariance_(1, 1) = (_data(1) == 0 ? 1e-6 : fabs(_data(1))) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor();
- data_covariance_(2, 2) = (_data(2) == 0 ? 1e-6 : fabs(_data(2))) * ((SensorOdom2D*) _sensor_ptr)->getRotationNoiseFactor();
+ data_covariance_(0, 0) = std::max(1e-6, fabs(_data(0)) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
+ data_covariance_(1, 1) = std::max(1e-6, fabs(_data(1)) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
+ data_covariance_(2, 2) = std::max(1e-6, fabs(_data(2)) * ((SensorOdom2D*) _sensor_ptr)->getRotationNoiseFactor());
// std::cout << data_covariance_ << std::endl;
}
@@ -44,7 +44,7 @@ Eigen::VectorXs CaptureOdom2D::computePrior() const
{
Eigen::Vector4s prior;
Eigen::Map<Eigen::Vector4s> initial_pose(getFramePtr()->getPPtr()->getPtr());
- ///std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
+ //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
prior(0) = initial_pose(0) + data_(0) * initial_pose(2) - data_(1) * initial_pose(3);
prior(1) = initial_pose(1) + data_(0) * initial_pose(3) + data_(1) * initial_pose(2);
prior(2) = initial_pose(2) * cos(data_(2)) - initial_pose(3) * sin(data_(2));
@@ -101,6 +101,9 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
data_(1) += (_new_capture->getData()(0) * sin(data_(2)) + _new_capture->getData()(1) * cos(data_(2)));
data_(2) += _new_capture->getData()(2);
data_covariance_ += _new_capture->getDataCovariance();
+
+ getFeatureListPtr()->front()->setMeasurement(data_);
+ getFeatureListPtr()->front()->setMeasurementCovariance(data_covariance_);
//std::cout << "Integrated odoms: " << std::endl << data_.transpose() << std::endl << data_covariance_ << std::endl;
}
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index dfee1b0c0e8b309e74be9d06bdb588718d154ecc..dfc720d395c7c130ca14c10481bb5b008032222a 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -38,6 +38,8 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
{
+ //std::cout << "computing covariances..." << std::endl;
+
// CREATE DESIRED COVARIANCES LIST
std::vector<std::pair<const double*, const double*>> covariance_blocks;
@@ -74,8 +76,11 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
Eigen::MatrixXs m_oo(current_orientation->getStateSize(),current_orientation->getStateSize());
Eigen::MatrixXs m_po(current_position->getStateSize(),current_orientation->getStateSize());
+ //std::cout << "getting m_pp covariance block... " << m_pp.rows() << "x" << m_pp.cols() << std::endl;
covariance_->GetCovarianceBlock(current_position_ptr, current_position_ptr, m_pp.data());
+ //std::cout << "getting m_oo covariance block... " << m_oo.rows() << "x" << m_oo.cols() << std::endl;
covariance_->GetCovarianceBlock(current_position_ptr, current_orientation_ptr, m_po.data());
+ //std::cout << "getting m_po covariance block... " << m_po.rows() << "x" << m_po.cols() << std::endl;
covariance_->GetCovarianceBlock(current_orientation_ptr, current_orientation_ptr, m_oo.data());
_problem_ptr->addCovarianceBlock(current_position, current_position, m_pp);
@@ -98,12 +103,19 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
Eigen::MatrixXs m_landmark_o_frame_p(landmark_orientation->getStateSize(),current_position->getStateSize());
Eigen::MatrixXs m_landmark_o_frame_o(landmark_orientation->getStateSize(),current_orientation->getStateSize());
+ //std::cout << "getting m_landmark_pp covariance block... " << m_landmark_pp.rows() << "x" << m_landmark_pp.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_position_ptr, m_landmark_pp.data());
+ //std::cout << "getting m_landmark_po covariance block... " << m_landmark_po.rows() << "x" << m_landmark_po.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_orientation_ptr, m_landmark_po.data());
+ //std::cout << "getting m_landmark_oo covariance block... " << m_landmark_oo.rows() << "x" << m_landmark_oo.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_orientation_ptr, landmark_orientation_ptr, m_landmark_oo.data());
+ //std::cout << "getting m_landmark_p_frame_p covariance block... " << m_landmark_p_frame_p.rows() << "x" << m_landmark_p_frame_p.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_position_ptr, current_position_ptr, m_landmark_p_frame_p.data());
+ //std::cout << "getting m_landmark_p_frame_o covariance block... " << m_landmark_p_frame_o.rows() << "x" << m_landmark_p_frame_o.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_position_ptr, current_orientation_ptr, m_landmark_p_frame_o.data());
+ //std::cout << "getting m_landmark_o_frame_p covariance block... " << m_landmark_o_frame_p.rows() << "x" << m_landmark_o_frame_p.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_position_ptr, m_landmark_o_frame_p.data());
+ //std::cout << "getting m_landmark_o_frame_o covariance block... " << m_landmark_o_frame_o.rows() << "x" << m_landmark_o_frame_o.cols() << std::endl;
covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_orientation_ptr, m_landmark_o_frame_o.data());
_problem_ptr->addCovarianceBlock(landmark_position, landmark_position, m_landmark_pp);
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h
index c4d8d91ce699035b4c7e72225cddce1a7ea27ce5..3cc76af752fcaf3f2c6b6b427088879cec46e83a 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D_theta.h
@@ -30,6 +30,31 @@ class ConstraintOdom2DTheta : public ConstraintSparse<3, 2, 1, 2, 1>
template<typename T>
bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
{
+// std::cout << "_p1: ";
+// for (int i=0; i < 2; i++)
+// std::cout << "\n\t" << _p1[i];
+// std::cout << std::endl;
+// std::cout << "_o1: ";
+// for (int i=0; i < 1; i++)
+// std::cout << "\n\t" << _o1[i];
+// std::cout << std::endl;
+// std::cout << "_p2: ";
+// for (int i=0; i < 2; i++)
+// std::cout << "\n\t" << _p2[i];
+// std::cout << std::endl;
+// std::cout << "_o2: ";
+// for (int i=0; i < 1; i++)
+// std::cout << "\n\t" << _o2[i];
+// std::cout << std::endl;
+// std::cout << "measurement_: ";
+// for (int i=0; i < 3; i++)
+// std::cout << "\n\t" << measurement_(i);
+// std::cout << std::endl;
+// std::cout << "measurement_covariance_: ";
+// for (int i=0; i < 3; i++)
+// std::cout << "\n\t" << measurement_covariance_(i,i);
+// std::cout << std::endl;
+
// Expected measurement
// rotar per menys l'angle de primer _o1
T expected_longitudinal = cos(_o1[0]) * (_p2[0] - _p1[0]) + sin(_o1[0]) * (_p2[1] - _p1[1]); // cos(-o1)(x2-x1) - sin(-o1)(y2-y1)
@@ -37,9 +62,9 @@ class ConstraintOdom2DTheta : public ConstraintSparse<3, 2, 1, 2, 1>
T expected_rotation = _o2[0] - _o1[0];
// Residuals
- _residuals[0] = (expected_longitudinal - T(measurement_(0))) / T(sqrt(measurement_covariance_(0, 0)));
- _residuals[1] = (expected_lateral - T(measurement_(1))) / T(sqrt(measurement_covariance_(1, 1)));
- _residuals[2] = (expected_rotation - T(measurement_(2))) / T(sqrt(measurement_covariance_(2, 2)));
+ _residuals[0] = (expected_longitudinal - T(measurement_(0))) / T(sqrt(std::max(measurement_covariance_(0, 0),1e-6)));
+ _residuals[1] = (expected_lateral - T(measurement_(1))) / T(sqrt(std::max(measurement_covariance_(1, 1),1e-6)));
+ _residuals[2] = (expected_rotation - T(measurement_(2))) / T(sqrt(std::max(measurement_covariance_(2, 2),1e-6)));
// Expected measurement
// T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index 94bc5fe417a4ed2ab706f6aed020dbc11267f246..70755c96d1c41739339bd61c130c05aa63e96ec5 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -29,7 +29,6 @@ class ConstraintSparse: public ConstraintBase
{
protected:
std::vector<StateBase*> state_ptr_vector_;
- std::vector<WolfScalar*> state_block_ptr_vector_;
std::vector<unsigned int> state_block_sizes_vector_;
public:
@@ -45,68 +44,6 @@ class ConstraintSparse: public ConstraintBase
static const unsigned int block8Size = BLOCK_8_SIZE;
static const unsigned int block9Size = BLOCK_9_SIZE;
- /** \brief Contructor with state pointer array
- *
- * Constructor with state pointer array
- * JVN: Potser aquest constructor no l'utilitzarem mai.. no?
- *
- **/
-// ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, WolfScalar** _blockPtrArray) :
-// ConstraintBase(_ftr_ptr,_tp),
-// state_block_ptr_vector_(10),
-// state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
-// {
-// for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
-// {
-// if (state_block_sizes_vector_.at(ii) != 0)
-// {
-// state_block_ptr_vector_.at(ii) = _blockPtrArray[ii];
-// }
-// else //at the end the vector is cropped to just relevant components
-// {
-// state_block_ptr_vector_.resize(ii);
-// break;
-// }
-// }
-// }
-//
-// /** \brief Contructor with state pointer separated
-// *
-// * Constructor with state pointers separated
-// *
-// **/
-// ConstraintSparse(FeatureBase* _ftr_ptr,
-// ConstraintType _tp,
-// WolfScalar* _state0Ptr,
-// WolfScalar* _state1Ptr = nullptr,
-// WolfScalar* _state2Ptr = nullptr,
-// WolfScalar* _state3Ptr = nullptr,
-// WolfScalar* _state4Ptr = nullptr,
-// WolfScalar* _state5Ptr = nullptr,
-// WolfScalar* _state6Ptr = nullptr,
-// WolfScalar* _state7Ptr = nullptr,
-// WolfScalar* _state8Ptr = nullptr,
-// WolfScalar* _state9Ptr = nullptr ) :
-// ConstraintBase(_ftr_ptr,_tp),
-// state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
-// state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
-// {
-// for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
-// {
-// if ( (state_block_ptr_vector_.at(ii) == nullptr) && (state_block_sizes_vector_.at(ii) == 0) )
-// {
-// state_block_sizes_vector_.resize(ii);
-// state_block_ptr_vector_.resize(ii);
-// break;
-// }
-// else // check error cases
-// {
-// assert(state_block_ptr_vector_.at(ii) != nullptr);
-// assert(state_block_sizes_vector_.at(ii) != 0);
-// }
-// }
-// }
-
/** \brief Contructor with state pointer separated
*
* Constructor with state pointers separated
@@ -126,22 +63,18 @@ class ConstraintSparse: public ConstraintBase
StateBase* _state9Ptr = nullptr ) :
ConstraintBase(_ftr_ptr,_tp),
state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- state_block_ptr_vector_({_state0Ptr->getPtr(), nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}),
state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
{
for (unsigned int ii = 1; ii<state_block_sizes_vector_.size(); ii++)
{
if (state_ptr_vector_.at(ii) != nullptr)
- {
assert(state_block_sizes_vector_.at(ii) != 0 && "Too many non-null state pointers in ConstraintSparse constructor");
- state_block_ptr_vector_.at(ii) = state_ptr_vector_.at(ii)->getPtr();
- }
+
else
{
assert(state_block_sizes_vector_.at(ii) == 0 && "No non-null state pointers enough in ConstraintSparse constructor");
state_ptr_vector_.resize(ii);
state_block_sizes_vector_.resize(ii);
- state_block_ptr_vector_.resize(ii);
break;
}
}
@@ -164,7 +97,96 @@ class ConstraintSparse: public ConstraintBase
**/
virtual const std::vector<WolfScalar*> getStateBlockPtrVector()
{
- return state_block_ptr_vector_;
+ assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
+
+ switch (state_ptr_vector_.size())
+ {
+ case 1:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()});
+ }
+ case 2:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr()});
+ }
+ case 3:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr()});
+ }
+ case 4:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr()});
+ }
+ case 5:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr()});
+ }
+ case 6:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr()});
+ }
+ case 7:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr()});
+ }
+ case 8:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr(),
+ state_ptr_vector_[7]->getPtr()});
+ }
+ case 9:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr(),
+ state_ptr_vector_[7]->getPtr(),
+ state_ptr_vector_[8]->getPtr()});
+ }
+ case 10:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr(),
+ state_ptr_vector_[7]->getPtr(),
+ state_ptr_vector_[8]->getPtr(),
+ state_ptr_vector_[9]->getPtr()});
+ }
+ }
}
/** \brief Returns a vector of pointers to the states
@@ -195,7 +217,7 @@ class ConstraintSparse: public ConstraintBase
printTabs(_ntabs);
_ost << "block " << ii << ": ";
for (unsigned int jj = 0; jj<state_block_sizes_vector_.at(ii); jj++)
- _ost << *(state_block_ptr_vector_.at(ii)+jj) << " ";
+ _ost << *(state_ptr_vector_.at(ii)->getPtr()+jj) << " ";
_ost << std::endl;
}
}
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 92c06eb368f120eee7001463feb87f8046e80ab4..5ec7d26e1eef8e298a0623d9afe56eb84ece33c4 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -178,7 +178,7 @@ int main(int argc, char** argv)
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager* wolf_manager = new WolfManager(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3, complex_angle);
+ WolfManager<StatePoint2D, StateTheta>* wolf_manager = new WolfManager<StatePoint2D, StateTheta>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
//std::cout << "START TRAJECTORY..." << std::endl;
// START TRAJECTORY ============================================================================================
@@ -232,33 +232,33 @@ int main(int argc, char** argv)
mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// ADD CAPTURES ---------------------------
- //std::cout << "ADD CAPTURES..." << std::endl;
+ std::cout << "ADD CAPTURES..." << std::endl;
t1 = clock();
// adding new sensor captures
wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
-// wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
- wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
- wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
+ wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+// wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+// wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
wolf_manager->update();
mean_times(1) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// UPDATING CERES ---------------------------
- //std::cout << "UPDATING CERES..." << std::endl;
+ std::cout << "UPDATING CERES..." << std::endl;
t1 = clock();
// update state units and constraints in ceres
ceres_manager->update(wolf_manager->getProblemPtr());
mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// SOLVE OPTIMIZATION ---------------------------
- //std::cout << "SOLVING..." << std::endl;
+ std::cout << "SOLVING..." << std::endl;
t1 = clock();
ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
//std::cout << summary.FullReport() << std::endl;
mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// COMPUTE COVARIANCES ---------------------------
- //std::cout << "COMPUTING COVARIANCES..." << std::endl;
+ std::cout << "COMPUTING COVARIANCES..." << std::endl;
t1 = clock();
ceres_manager->computeCovariances(wolf_manager->getProblemPtr());
mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/test_iQR_wolf2.cpp
index d2d9bfdd903580291a317cbff041cf386d08ed4c..a0852c250ed109cbe9fbd012fa4a59a3d30a53b7 100644
--- a/src/examples/test_iQR_wolf2.cpp
+++ b/src/examples/test_iQR_wolf2.cpp
@@ -26,8 +26,10 @@
#include "constraint_base.h"
#include "wolf_manager.h"
-// ccolamd
+// wolf solver
#include "solver/ccolamd_ordering.h"
+#include "solver/cost_function_base.h"
+#include "solver/cost_function_sparse.h"
//C includes for sleep, time and main args
#include "unistd.h"
@@ -132,24 +134,6 @@ void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const
original.coeffRef(r + row, c + col) += ins(r,c);
}
-struct node
-{
- public:
- int id;
- int dim;
- int location;
- int order;
-
- node(const int _id, const int _dim, const int _location, const int _order) :
- id(_id),
- dim(_dim),
- location(_location),
- order(_order)
- {
-
- }
-};
-
struct measurement
{
public:
@@ -158,24 +142,6 @@ struct measurement
int dim;
int location;
- measurement(const int _idx1, const VectorXd &_error, const int _meas_dim) :
- nodes_idx({_idx1}),
- error(_error),
- dim(_meas_dim),
- location(0)
- {
- //jacobians.push_back(_jacobian1);
- }
-
- measurement(const int _idx1, const int _idx2, const VectorXd &_error, const int _meas_dim) :
- nodes_idx({_idx1, _idx2}),
- error(_error),
- dim(_meas_dim),
- location(0)
- {
-
- }
-
measurement(const std::vector<int> & _idxs, const VectorXd &_error, const int _meas_dim) :
nodes_idx(_idxs),
error(_error),
@@ -195,18 +161,20 @@ class SolverQR
VectorXd b_, x_incr_;
int n_measurements;
int n_nodes_;
- std::vector<node> nodes_;
+ //std::vector<node> nodes_;
std::vector<StateBase*> states_;
std::vector<measurement> measurements_;
std::vector<ConstraintBase*> constraints_;
+ std::vector<CostFunctionBase*> cost_functions_;
// ordering
SparseMatrix<int> A_nodes_;
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_;
- std::map<int, int> id_2_idx;
+ std::map<int, int> id_2_idx_;
CCOLAMDOrdering<int> ordering_;
VectorXi nodes_ordering_constraints_;
+ ArrayXi node_locations_;
int n_new_measurements_;
// time
@@ -224,11 +192,12 @@ class SolverQR
n_nodes_(0),
A_nodes_(0,0),
acc_permutation_nodes_(0),
+ n_new_measurements_(0),
time_ordering_(0),
time_solving_(0),
time_managing_(0)
{
- //
+ node_locations_.resize(0);
}
virtual ~SolverQR()
@@ -296,17 +265,18 @@ class SolverQR
std::cout << "adding state unit " << _state_ptr->nodeId() << std::endl;
if (_state_ptr->getStateStatus() == ST_ESTIMATED)
{
- states_.push_back(_state_ptr);
- id_2_idx[_state_ptr->nodeId()] = states_.size()-1;
-
- std::cout << "idx " << id_2_idx[_state_ptr->nodeId()] << std::endl;
t_managing_ = clock();
unsigned int node_dim = _state_ptr->getStateSize();
int node_idx= _state_ptr->nodeId();
n_nodes_++;
- nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
+ states_.push_back(_state_ptr);
+ id_2_idx_[_state_ptr->nodeId()] = states_.size()-1;
+
+ std::cout << "idx " << id_2_idx_[_state_ptr->nodeId()] << std::endl;
+
+ //nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
// Resize accumulated permutations
augment_permutation(acc_permutation_nodes_, n_nodes_);
@@ -329,40 +299,27 @@ class SolverQR
//TODO
}
- void getResidualJacobians(const ConstraintBase* _constraint_ptr, VectorXs &error, std::vector<MatrixXs>& jacobians)
- {
- assert(error.size() == _constraint_ptr->getSize());
-
- if (!jacobians.empty())
- jacobians.clear();
-
- //TODO get the jacobian calling functor operator()
- for (int i = 0; i < _constraint_ptr->getStatePtrVector().size(); i++)
- if (_constraint_ptr->getStatePtrVector().at(i)->getStateStatus() == ST_ESTIMATED)
- jacobians.push_back(MatrixXs::Constant(_constraint_ptr->getSize(), _constraint_ptr->getStatePtrVector().at(i)->getStateSize(), 0.1) + MatrixXs::Identity(_constraint_ptr->getSize(), _constraint_ptr->getStatePtrVector().at(i)->getStateSize()));
-
- // TODO get the error calling functor operator()
- error = VectorXs::LinSpaced(_constraint_ptr->getSize(), _constraint_ptr->getSize() * _constraint_ptr->nodeId(), _constraint_ptr->getSize() * _constraint_ptr->nodeId()+_constraint_ptr->getSize()-1);
- }
-
void addConstraint(ConstraintBase* _constraint_ptr)
{
std::cout << "adding constraint " << _constraint_ptr->nodeId() << std::endl;
t_managing_ = clock();
constraints_.push_back(_constraint_ptr);
+ cost_functions_.push_back(createCostFunction(_constraint_ptr));
int meas_dim = _constraint_ptr->getSize();
std::vector<MatrixXs> jacobians(_constraint_ptr->getStatePtrVector().size());
VectorXs error(_constraint_ptr->getSize());
- getResidualJacobians(_constraint_ptr, error, jacobians);
+ cost_functions_.back()->evaluateResidualJacobians();
+ cost_functions_.back()->getResidual(error);
+ cost_functions_.back()->getJacobians(jacobians);
std::vector<int> idxs;
for (int i = 0; i < _constraint_ptr->getStatePtrVector().size(); i++)
if (_constraint_ptr->getStatePtrVector().at(i)->getStateStatus() == ST_ESTIMATED)
- idxs.push_back(id_2_idx[_constraint_ptr->getStatePtrVector().at(i)->nodeId()]);
+ idxs.push_back(id_2_idx_[_constraint_ptr->getStatePtrVector().at(i)->nodeId()]);
measurement _meas(idxs, error, meas_dim);
@@ -379,15 +336,15 @@ class SolverQR
// ADD MEASUREMENTS
for (unsigned int j = 0; j < idxs.size(); j++)
{
- assert(acc_permutation_nodes_.indices()(idxs.at(j)) == nodes_.at(idxs.at(j)).order);
+ assert(acc_permutation_nodes_.indices()(idxs.at(j)) == node_order(idxs.at(j)));//nodes_.at(idxs.at(j)).order);
- int ordered_node = nodes_.at(idxs.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
+ //int ordered_node = nodes_.at(idxs.at(j)).order;
- add_sparse_block(jacobians.at(j), A_, A_.rows()-meas_dim, nodes_.at(idxs.at(j)).location);
+ add_sparse_block(jacobians.at(j), A_, A_.rows()-meas_dim, node_location(idxs.at(j)));//nodes_.at(idxs.at(j)).location);
- A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
+ A_nodes_.coeffRef(A_nodes_.rows()-1, node_order(idxs.at(j))) = 1;
- assert(jacobians.at(j).cols() == nodes_.at(idxs.at(j)).dim);
+ assert(jacobians.at(j).cols() == node_dim(idxs.at(j)));//nodes_.at(idxs.at(j)).dim);
assert(jacobians.at(j).rows() == meas_dim);
}
@@ -431,7 +388,7 @@ class SolverQR
// partial ordering
else
{
- int first_ordered_node = nodes_.at(_first_ordered_idx).order;
+ int first_ordered_node = node_order(_first_ordered_idx);//nodes_.at(_first_ordered_idx).order;
int ordered_nodes = n_nodes_ - first_ordered_node;
int unordered_nodes = n_nodes_ - ordered_nodes;
if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
@@ -450,7 +407,7 @@ class SolverQR
ordering_(sub_A_nodes_, partial_permutation_nodes, nodes_ordering_constraints_.data());
// node ordering to variable ordering
- int ordered_variables = A_.cols() - nodes_.at(_first_ordered_idx).location;
+ int ordered_variables = A_.cols() - node_location(_first_ordered_idx);//nodes_.at(_first_ordered_idx).location;
// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
// std::cout << "A_.cols() " << A_.cols() << std::endl;
// std::cout << "nodes_.at(_first_ordered_idx).location " << nodes_.at(_first_ordered_idx).location << std::endl;
@@ -482,18 +439,27 @@ class SolverQR
for (int i = 0; i < n_new_measurements_; i++)
{
ConstraintBase* ct_ptr = constraints_.at(constraints_.size()-1-i);
+ std::cout << "constraint: " << i << " id: " << constraints_.at(constraints_.size()-1-i)->nodeId() << std::endl;
for (int j = 0; j < ct_ptr->getStatePtrVector().size(); j++)
{
- int idx = id_2_idx[ct_ptr->getStatePtrVector().at(j)->nodeId()];
- if (first_ordered_node > nodes_.at(idx).order)
+ if (ct_ptr->getStatePtrVector().at(j)->getStateStatus() == ST_ESTIMATED)
{
- first_ordered_idx = idx;
- first_ordered_node = nodes_.at(idx).order;
+ int idx = id_2_idx_[ct_ptr->getStatePtrVector().at(j)->nodeId()];
+ std::cout << "estimated idx " << idx << std::endl;
+ std::cout << "node_order(idx) " << node_order(idx) << std::endl;
+ std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+ if (first_ordered_node > node_order(idx))//nodes_.at(idx).order)
+ {
+ first_ordered_idx = idx;
+ std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
+ first_ordered_node = node_order(idx);
+ std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+ }
}
}
}
-// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
-// std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
+ std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+ std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
bool batch = (mode !=2 || first_ordered_node == 0);
bool order = (mode !=0 && n_nodes_ > 1);
@@ -539,8 +505,8 @@ class SolverQR
// std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
- int ordered_variables = A_.cols() - nodes_.at(first_ordered_idx).location;
- int unordered_variables = nodes_.at(first_ordered_idx).location;
+ int ordered_variables = A_.cols() - node_location(first_ordered_idx);//nodes_.at(first_ordered_idx).location;
+ int unordered_variables = node_location(first_ordered_idx);//nodes_.at(first_ordered_idx).location;
SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
solver_.compute(A_partial);
@@ -577,10 +543,19 @@ class SolverQR
}
}
- // UNDO ORDERING FOR RESULT
- PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
- permutation_2_block_permutation(acc_permutation_nodes_, acc_permutation); // TODO via pointers
- x_incr_ = acc_permutation.inverse() * x_incr_;
+// // UNDO ORDERING FOR RESULT
+// PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
+// permutation_2_block_permutation(acc_permutation_nodes_, acc_permutation); // TODO via pointers
+// x_incr_ = acc_permutation.inverse() * x_incr_;
+
+ // UPDATE X VALUE
+ for (unsigned int i = 0; i<states_.size(); i++)
+ {
+ Map<VectorXs> x_i(states_.at(i)->getPtr(), states_.at(i)->getStateSize());
+ x_i += x_incr_.segment(node_location(i), states_.at(i)->getStateSize());
+ }
+
+
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
n_new_measurements_ = 0;
@@ -591,28 +566,26 @@ class SolverQR
void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
{
//std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl;
- ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
+ perm_nodes_2_locations(_perm_nodes, node_locations_);
//std::cout << "locations: " << locations.transpose() << std::endl;
//std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
int last_idx = 0;
- for (unsigned int i = 0; i<locations.size(); i++)
+ for (unsigned int i = 0; i<node_locations_.size(); i++)
{
- perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1);
- last_idx += nodes_.at(i).dim;
+ perm_variables.indices().segment(last_idx, node_dim(i)) = VectorXi::LinSpaced(node_dim(i), node_locations_(i), node_locations_(i)+node_dim(i)-1);
+ last_idx += node_dim(i);
//std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl;
}
//std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
}
- ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes)
+ void perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, ArrayXi& locations)
{
- ArrayXi indices = _perm_nodes.indices().array();
-
- for (unsigned int i = 0; i<indices.size(); i++)
- indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices);
+ locations = _perm_nodes.indices().array();
- return indices;
+ for (unsigned int i = 0; i<locations.size(); i++)
+ locations = (locations > locations(i)).select(locations + node_dim(i)-1, locations);
}
void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
@@ -624,6 +597,12 @@ class SolverQR
new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
perm.resize(new_size);
perm.indices() = new_indices;
+ std::cout << "permutation augmented" << std::endl;
+
+ // resize and update locations
+ node_locations_.conservativeResize(node_locations_.size() + 1);
+ node_locations_(node_locations_.size()-1) = x_incr_.size();
+ std::cout << "node_locations_ augmented" << std::endl;
}
void accumulate_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
@@ -645,11 +624,109 @@ class SolverQR
//std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
// update nodes orders and locations
- ArrayXi locations = perm_nodes_2_locations(acc_permutation_nodes_);
- for (int i = 0; i < nodes_.size(); i++)
+ perm_nodes_2_locations(acc_permutation_nodes_, node_locations_);
+
+// for (int i = 0; i < nodes_.size(); i++)
+// {
+// nodes_.at(i).order = acc_permutation_nodes_.indices()(i);
+// nodes_.at(i).location = node_locations_(i);
+// }
+ }
+
+ int node_dim(const int _idx)
+ {
+ return states_.at(_idx)->getStateSize();
+ }
+
+ int node_order(const int _idx)
+ {
+ return acc_permutation_nodes_.indices()(_idx);
+ }
+
+ int node_location(const int _idx)
+ {
+ return node_locations_(_idx);
+ }
+
+ CostFunctionBase* createCostFunction(ConstraintBase* _corrPtr)
+ {
+ //std::cout << "adding ctr " << _corrPtr->nodeId() << std::endl;
+ //_corrPtr->print();
+
+ switch (_corrPtr->getConstraintType())
{
- nodes_.at(i).order = acc_permutation_nodes_.indices()(i);
- nodes_.at(i).location = locations(i);
+ case CTR_GPS_FIX_2D:
+ {
+ ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
+ return (CostFunctionBase*)(new CostFunctionSparse<ConstraintGPS2D,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr));
+ break;
+ }
+ case CTR_ODOM_2D_COMPLEX_ANGLE:
+ {
+ ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
+ return (CostFunctionBase*)new CostFunctionSparse<ConstraintOdom2DComplexAngle,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_ODOM_2D_THETA:
+ {
+ ConstraintOdom2DTheta* specific_ptr = (ConstraintOdom2DTheta*)(_corrPtr);
+ return (CostFunctionBase*)new CostFunctionSparse<ConstraintOdom2DTheta,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_CORNER_2D_THETA:
+ {
+ ConstraintCorner2DTheta* specific_ptr = (ConstraintCorner2DTheta*)(_corrPtr);
+ return (CostFunctionBase*)new CostFunctionSparse<ConstraintCorner2DTheta,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
+
+ return nullptr;
}
}
@@ -753,7 +830,7 @@ int main(int argc, char *argv[])
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager* wolf_manager = new WolfManager(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10);
+ WolfManager<StatePoint2D, StateTheta>* wolf_manager = new WolfManager<StatePoint2D, StateTheta>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
@@ -811,7 +888,7 @@ int main(int argc, char *argv[])
//std::cout << "ADD CAPTURES..." << std::endl;
// adding new sensor captures
wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
-// wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+ wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
@@ -827,7 +904,7 @@ int main(int argc, char *argv[])
// SOLVE OPTIMIZATION ---------------------------
//std::cout << "SOLVING..." << std::endl;
- solver_unordered.solve(2);
+ solver_unordered.solve(0);
std::cout << "========================= RESULTS " << step << ":" << std::endl;
solver_unordered.print_results();
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 4d539d6752a2929c3795dbff4ca93f3e6b2d9a27..c0a40563e634ba1b2aa2c79351dfe6a262c48ae9 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -128,6 +128,12 @@ void FrameBase::setState(const Eigen::VectorXs& _st)
//std::cout << "setted state: " << *p_ptr_->getPtr() << " " << *(p_ptr_->getPtr()+1) << std::endl;
}
+Eigen::Map<Eigen::VectorXs> FrameBase::getState() const
+{
+ return Eigen::Map<Eigen::VectorXs>(p_ptr_->getPtr(),
+ p_ptr_->getStateSize() + o_ptr_->getStateSize() + v_ptr_->getStateSize() + w_ptr_->getStateSize());
+}
+
void FrameBase::addCapture(CaptureBase* _capt_ptr)
{
addDownNode(_capt_ptr);
@@ -212,6 +218,15 @@ StateBase* FrameBase::getWPtr() const
return w_ptr_;
}
+CaptureBaseIter FrameBase::hasCaptureOf(const SensorBase* _sensor_ptr)
+{
+ for (auto capture_it = getCaptureListPtr()->begin(); capture_it != getCaptureListPtr()->end(); capture_it++)
+ if ((*capture_it)->getSensorPtr() == _sensor_ptr)
+ return capture_it;
+
+ return getCaptureListPtr()->end();
+}
+
void FrameBase::printSelf(unsigned int _ntabs, std::ostream& _ost) const
{
NodeLinked::printSelf(_ntabs, _ost);
diff --git a/src/frame_base.h b/src/frame_base.h
index def3aea2d1dc8a46624201576e99773ce59c377b..fec673a1237ed3f720d838de1a8c7be337362f02 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -95,6 +95,8 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
void addCapture(CaptureBase* _capt_ptr);
+ Eigen::Map<Eigen::VectorXs> getState() const;
+
void removeCapture(CaptureBaseIter& _capt_ptr);
TrajectoryBase* getTrajectoryPtr() const;
@@ -117,6 +119,8 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
const Eigen::Matrix4s * getTransformationMatrix() const; //ACM: Who owns this return matrix ?
+ CaptureBaseIter hasCaptureOf(const SensorBase* _sensor_ptr);
+
virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
};
diff --git a/src/solver/cost_function_base.h b/src/solver/cost_function_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..51341de1f056eb76999c3f96c7548083374eaecd
--- /dev/null
+++ b/src/solver/cost_function_base.h
@@ -0,0 +1,98 @@
+/*
+ * cost_function_base.h
+ *
+ * Created on: Jun 25, 2015
+ * Author: jvallve
+ */
+
+#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
+#define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
+
+#include "wolf.h"
+
+class CostFunctionBase
+{
+ protected:
+ int n_blocks_;
+ Eigen::MatrixXs J_0_;
+ Eigen::MatrixXs J_1_;
+ Eigen::MatrixXs J_2_;
+ Eigen::MatrixXs J_3_;
+ Eigen::MatrixXs J_4_;
+ Eigen::MatrixXs J_5_;
+ Eigen::MatrixXs J_6_;
+ Eigen::MatrixXs J_7_;
+ Eigen::MatrixXs J_8_;
+ Eigen::MatrixXs J_9_;
+ Eigen::VectorXs residual_;
+ std::vector<Eigen::MatrixXs*> jacobians_;
+ std::vector<unsigned int> block_sizes_;
+
+ public:
+ CostFunctionBase(const unsigned int &_measurement_size,
+ const unsigned int &_block_0_size,
+ const unsigned int &_block_1_size,
+ const unsigned int &_block_2_size,
+ const unsigned int &_block_3_size,
+ const unsigned int &_block_4_size,
+ const unsigned int &_block_5_size,
+ const unsigned int &_block_6_size,
+ const unsigned int &_block_7_size,
+ const unsigned int &_block_8_size,
+ const unsigned int &_block_9_size) :
+ n_blocks_(10),
+ J_0_(_measurement_size, _block_0_size),
+ J_1_(_measurement_size, _block_1_size),
+ J_2_(_measurement_size, _block_2_size),
+ J_3_(_measurement_size, _block_3_size),
+ J_4_(_measurement_size, _block_4_size),
+ J_5_(_measurement_size, _block_5_size),
+ J_6_(_measurement_size, _block_6_size),
+ J_7_(_measurement_size, _block_7_size),
+ J_8_(_measurement_size, _block_8_size),
+ J_9_(_measurement_size, _block_9_size),
+ residual_(_measurement_size),
+ jacobians_({&J_0_,&J_1_,&J_2_,&J_3_,&J_4_,&J_5_,&J_6_,&J_7_,&J_8_,&J_9_}),
+ block_sizes_({_block_0_size, _block_1_size, _block_2_size, _block_3_size, _block_4_size, _block_5_size, _block_6_size, _block_7_size, _block_8_size, _block_9_size})
+ {
+ int last_jet_idx = 0;
+ for (unsigned int i = 1; i<n_blocks_; i++)
+ {
+ if (block_sizes_.at(i) == 0)
+ {
+ n_blocks_ = i;
+ jacobians_.resize(n_blocks_);
+ block_sizes_.resize(n_blocks_);
+ break;
+ }
+ }
+ }
+
+ virtual ~CostFunctionBase()
+ {
+
+ }
+
+ virtual void evaluateResidualJacobians() = 0;
+
+
+ void getResidual(Eigen::VectorXs &residual)
+ {
+ residual.resize(residual_.size());
+ residual = residual_;
+ }
+
+ std::vector<Eigen::MatrixXs*> getJacobians()
+ {
+ return jacobians_;
+ }
+
+ void getJacobians(std::vector<Eigen::MatrixXs>& jacobians)
+ {
+ jacobians.resize(n_blocks_);
+ for (int i = 0; i<n_blocks_; i++)
+ jacobians.at(i) = (*jacobians_.at(i));
+ }
+};
+
+#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_ */
diff --git a/src/solver/cost_function_sparse.h b/src/solver/cost_function_sparse.h
new file mode 100644
index 0000000000000000000000000000000000000000..77ebf13f682c331ae99644162d86bb400ab71bc8
--- /dev/null
+++ b/src/solver/cost_function_sparse.h
@@ -0,0 +1,556 @@
+#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
+#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
+
+//wolf includes
+#include "wolf.h"
+#include "cost_function_sparse_base.h"
+
+// CERES JET
+#include "ceres/jet.h"
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+class CostFunctionSparse : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_);
+ }
+
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ 0,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ 0,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ 0,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_);
+ }
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE>
+class CostFunctionSparse<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0> : CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>
+{
+ public:
+ CostFunctionSparse(ConstraintT* _constraint_ptr) :
+ CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0>(_constraint_ptr)
+ {
+
+ }
+
+ void callFunctor()
+ {
+ (*this->constraint_ptr_)(this->jets_0_,this->residuals_jet_);
+ }
+};
+
+#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_ */
diff --git a/src/solver/cost_function_sparse_base.h b/src/solver/cost_function_sparse_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c17472ff1f2295ecaa671ab56ebca813867e7f1
--- /dev/null
+++ b/src/solver/cost_function_sparse_base.h
@@ -0,0 +1,320 @@
+/*
+ * cost_function_sparse.h
+ *
+ * Created on: Jun 25, 2015
+ * Author: jvallve
+ */
+
+#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
+#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
+
+//wolf includes
+#include "wolf.h"
+#include "cost_function_base.h"
+
+// CERES JET
+#include "ceres/jet.h"
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE = 0,
+ unsigned int BLOCK_2_SIZE = 0,
+ unsigned int BLOCK_3_SIZE = 0,
+ unsigned int BLOCK_4_SIZE = 0,
+ unsigned int BLOCK_5_SIZE = 0,
+ unsigned int BLOCK_6_SIZE = 0,
+ unsigned int BLOCK_7_SIZE = 0,
+ unsigned int BLOCK_8_SIZE = 0,
+ unsigned int BLOCK_9_SIZE = 0>
+class CostFunctionSparseBase : CostFunctionBase
+{
+ typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE +
+ BLOCK_1_SIZE +
+ BLOCK_2_SIZE +
+ BLOCK_3_SIZE +
+ BLOCK_4_SIZE +
+ BLOCK_5_SIZE +
+ BLOCK_6_SIZE +
+ BLOCK_7_SIZE +
+ BLOCK_8_SIZE +
+ BLOCK_9_SIZE> WolfJet;
+ protected:
+ ConstraintT* constraint_ptr_;
+ WolfJet jets_0_[BLOCK_0_SIZE];
+ WolfJet jets_1_[BLOCK_1_SIZE];
+ WolfJet jets_2_[BLOCK_2_SIZE];
+ WolfJet jets_3_[BLOCK_3_SIZE];
+ WolfJet jets_4_[BLOCK_4_SIZE];
+ WolfJet jets_5_[BLOCK_5_SIZE];
+ WolfJet jets_6_[BLOCK_6_SIZE];
+ WolfJet jets_7_[BLOCK_7_SIZE];
+ WolfJet jets_8_[BLOCK_8_SIZE];
+ WolfJet jets_9_[BLOCK_9_SIZE];
+ WolfJet residuals_jet_[MEASUREMENT_SIZE];
+
+ public:
+
+ /** \brief Constructor with constraint pointer
+ *
+ * Constructor with constraint pointer
+ *
+ */
+ CostFunctionSparseBase(ConstraintT* _constraint_ptr);
+
+ /** \brief Default destructor
+ *
+ * Default destructor
+ *
+ */
+ virtual ~CostFunctionSparseBase();
+
+ /** \brief Evaluate residuals and jacobians of the constraint in the current x
+ *
+ * Evaluate residuals and jacobians of the constraint in the current x
+ *
+ */
+ virtual void evaluateResidualJacobians();
+
+ protected:
+
+ /** \brief Calls the functor of the constraint evaluating jets
+ *
+ * Calls the functor of the constraint evaluating jets
+ *
+ */
+ virtual void callFunctor() = 0;
+
+ /** \brief Initialize the infinitesimal part of jets
+ *
+ * Initialize the infinitesimal part of jets with zeros and ones
+ *
+ */
+ void initializeJets();
+
+ /** \brief Gets the evaluation point
+ *
+ * Gets the evaluation point from the state
+ *
+ */
+ void evaluateX();
+};
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::CostFunctionSparseBase(ConstraintT* _constraint_ptr) :
+ CostFunctionBase(MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE, BLOCK_5_SIZE, BLOCK_6_SIZE, BLOCK_7_SIZE, BLOCK_8_SIZE,BLOCK_9_SIZE),
+ constraint_ptr_(_constraint_ptr)
+{
+ initializeJets();
+}
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::~CostFunctionSparseBase()
+{
+
+}
+
+template <class ConstraintT,
+ const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+void CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::evaluateResidualJacobians()
+{
+ evaluateX();
+
+ callFunctor();
+
+ // fill the jacobian matrices
+ int jacobian_location = 0;
+ for (int i = 0; i<n_blocks_; i++)
+ {
+ for (int row = 0; row < MEASUREMENT_SIZE; row++)
+ jacobians_.at(i)->row(row) = residuals_jet_[row].v.segment(jacobian_location, block_sizes_.at(i));
+ jacobian_location += block_sizes_.at(i);
+ std::cout << "filled jacobian " << i << ":" << std::endl << (*jacobians_.at(i)) << std::endl;
+ }
+
+ // fill the residual vector
+ for (int i = 0; i < MEASUREMENT_SIZE; i++)
+ residual_(i) = residuals_jet_[i].a;
+}
+
+template <class ConstraintT,
+const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ void CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::initializeJets()
+{
+ int last_jet_idx = 0;
+ // JET 0
+ for (int i = 0; i < BLOCK_0_SIZE; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ // JET 1
+ for (int i = 0; i < BLOCK_1_SIZE; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ // JET 2
+ for (int i = 0; i < BLOCK_2_SIZE; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ // JET 3
+ for (int i = 0; i < BLOCK_3_SIZE; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ // JET 4
+ for (int i = 0; i < BLOCK_4_SIZE; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ // JET 5
+ for (int i = 0; i < BLOCK_5_SIZE; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ // JET 6
+ for (int i = 0; i < BLOCK_6_SIZE; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ // JET 7
+ for (int i = 0; i < BLOCK_7_SIZE; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ // JET 8
+ for (int i = 0; i < BLOCK_8_SIZE; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ // JET 9
+ for (int i = 0; i < BLOCK_9_SIZE; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+}
+
+template <class ConstraintT,
+const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ void CostFunctionSparseBase<ConstraintT,
+ MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::evaluateX()
+{
+ // JET 0
+ for (int i = 0; i < BLOCK_0_SIZE; i++)
+ jets_0_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(0)+i);
+ // JET 1
+ for (int i = 0; i < BLOCK_1_SIZE; i++)
+ jets_1_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(1)+i);
+ // JET 2
+ for (int i = 0; i < BLOCK_2_SIZE; i++)
+ jets_2_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(2)+i);
+ // JET 3
+ for (int i = 0; i < BLOCK_3_SIZE; i++)
+ jets_3_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(3)+i);
+ // JET 4
+ for (int i = 0; i < BLOCK_4_SIZE; i++)
+ jets_4_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(4)+i);
+ // JET 5
+ for (int i = 0; i < BLOCK_5_SIZE; i++)
+ jets_5_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(5)+i);
+ // JET 6
+ for (int i = 0; i < BLOCK_6_SIZE; i++)
+ jets_6_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(6)+i);
+ // JET 7
+ for (int i = 0; i < BLOCK_7_SIZE; i++)
+ jets_7_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(7)+i);
+ // JET 8
+ for (int i = 0; i < BLOCK_8_SIZE; i++)
+ jets_8_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(8)+i);
+ // JET 9
+ for (int i = 0; i < BLOCK_9_SIZE; i++)
+ jets_9_[i].a = *(constraint_ptr_->getStateBlockPtrVector().at(9)+i);
+}
+
+#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_ */
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 5cbf0036ec6abf2a02d5972002bed33729b3206d..c76955a382900cd7c37473bff3118cf4b8c84ace 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -26,6 +26,11 @@ FrameBaseList* TrajectoryBase::getFrameListPtr()
return getDownNodeListPtr();
}
+FrameBase* TrajectoryBase::getLastFramePtr()
+{
+ return getDownNodeListPtr()->back();
+}
+
void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
{
for(auto fr_it = getFrameListPtr()->begin(); fr_it != getFrameListPtr()->end(); ++fr_it)
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index d7100f61e04e74b92432905463fc23b6e3c45629..709c164c996d5eba3f96f98fc21306288d7752ab 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -63,6 +63,14 @@ class TrajectoryBase : public NodeLinked<WolfProblem,FrameBase>
**/
FrameBaseList* getFrameListPtr();
+ /** \brief Returns a pointer to last Frame
+ *
+ * Returns a pointer to last Frame
+ *
+ **/
+ FrameBase* getLastFramePtr();
+
+
/** \brief Returns a list of all constraints in the trajectory thru reference
*
* Returns a list of all constraints in the trajectory thru reference
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index dda55e34984cbf0b532787dcbd8bf93aeb2b483e..d220ff91ce642e1dd3def10c2da22c41575d16e5 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -40,6 +40,7 @@
#include "wolf_problem.h"
#include "state_quaternion.h"
+template <class StatePositionT, class StateOrientationT>
class WolfManager
{
protected:
@@ -51,6 +52,8 @@ class WolfManager
//auxiliar/temporary iterators, frames and captures
FrameBaseIter first_window_frame_;
+ FrameBase* current_frame_;
+ FrameBase* last_frame_;
CaptureRelative* last_capture_relative_;
CaptureRelative* second_last_capture_relative_;
std::queue<CaptureBase*> new_captures_;
@@ -58,36 +61,48 @@ class WolfManager
//Manager parameters
unsigned int window_size_;
WolfScalar new_frame_elapsed_time_;
- bool use_complex_angles_;
public:
- WolfManager(const unsigned int& _state_length,
+ WolfManager(const unsigned int& _state_length,
SensorBase* _sensor_prior,
const Eigen::VectorXs& _init_frame,
const Eigen::MatrixXs& _init_frame_cov,
const unsigned int& _w_size = 10,
- const WolfScalar& _new_frame_elapsed_time = 0.1,
- const bool _complex_angle = false) :
+ const WolfScalar& _new_frame_elapsed_time = 0.1) :
problem_(new WolfProblem(_state_length)),
sensor_prior_(_sensor_prior),
+ current_frame_(nullptr),
+ last_frame_(nullptr),
last_capture_relative_(nullptr),
- second_last_capture_relative_(nullptr),
window_size_(_w_size),
- new_frame_elapsed_time_(_new_frame_elapsed_time),
- use_complex_angles_(_complex_angle)
+ new_frame_elapsed_time_(_new_frame_elapsed_time)
{
- assert( ((!_complex_angle && _init_frame.size() == 3 && _init_frame_cov.cols() == 3 && _init_frame_cov.rows() == 3) ||
- (_complex_angle && _init_frame.size() == 4 && _init_frame_cov.cols() == 4 && _init_frame_cov.rows() == 4))
- && "Wrong init_frame state vector or covariance matrix size");
+ assert( _init_frame.size() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
+ _init_frame_cov.cols() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
+ _init_frame_cov.rows() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
+ "Wrong init_frame state vector or covariance matrix size");
+ std::cout << "initializing wolfmanager" << std::endl;
- // Set initial covariance with a fake ODOM 2D capture to a fixed frame
+ // Fake frame for prior
createFrame(_init_frame, TimeStamp(0));
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
- last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov)));
+ problem_->getLastFramePtr()->fix();
+ first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
+ // Initial frame
createFrame(_init_frame, TimeStamp(0));
- second_last_capture_relative_->processCapture();
+ first_window_frame_++;
+
+ // Initial covariance (fake ODOM 2D capture from fake frame to initial frame)
+ CaptureRelative* initial_covariance = new CaptureOdom2D(TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov);
+ last_frame_->addCapture(initial_covariance);
+ initial_covariance->processCapture();
+ last_capture_relative_ = initial_covariance;
+
+
+ // Current robot frame
+ createFrame(TimeStamp(0));
+ std::cout << " wolfmanager initialized" << std::endl;
}
virtual ~WolfManager()
@@ -97,111 +112,138 @@ class WolfManager
void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
{
- // Create frame and add it to the trajectory
- StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr());
- problem_->addState(new_position, _frame_state.head(2));
+ std::cout << "creating new frame..." << std::endl;
- StateBase* new_orientation;
- if (use_complex_angles_)
- new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
- else
- new_orientation = new StateTheta(problem_->getNewStatePtr());
+ // Store last frame
+ last_frame_ = current_frame_;
+ // Create frame and add it to the trajectory
+ StatePositionT* new_position = new StatePositionT(problem_->getNewStatePtr());
+ problem_->addState(new_position, _frame_state.head(new_position->getStateSize()));
+ //std::cout << "StatePosition created" << std::endl;
+
+ StateOrientationT* new_orientation = new StateOrientationT(problem_->getNewStatePtr());
problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
+ //std::cout << "StateOrientation created" << std::endl;
problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation));
+ //std::cout << "frame created" << std::endl;
+
+ // Store new current frame
+ current_frame_ = problem_->getLastFramePtr();
+ //std::cout << "current_frame_" << std::endl;
- // add a zero odometry capture (in order to integrate)
- CaptureOdom2D* empty_odom = new CaptureOdom2D(_time_stamp, sensor_prior_, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Zero());
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(empty_odom);
- second_last_capture_relative_ = last_capture_relative_;
- last_capture_relative_ = (CaptureRelative*) (empty_odom);
+ // no last capture relative
+ last_capture_relative_ = nullptr;
+ //std::cout << "last_frame_" << std::endl;
+
+ // Fixing or removing old frames
+ manage_window();
+ std::cout << "new frame created" << std::endl;
+ }
+
+ void createFrame(const TimeStamp& _time_stamp)
+ {
+ std::cout << "creating new frame from prior..." << std::endl;
+ createFrame(last_capture_relative_->computePrior(), _time_stamp);
}
void addCapture(CaptureBase* _capture)
{
new_captures_.push(_capture);
- //std::cout << "added new capture: " << _capture->nodeId() << "stamp: " << _capture->getTimeStamp().get() << std::endl;
+ std::cout << "added new capture: " << _capture->nodeId() << " stamp: ";
+ _capture->getTimeStamp().print();
+ std::cout << std::endl;
+ }
+ void manage_window()
+ {
+ std::cout << "managing window..." << std::endl;
+ // WINDOW of FRAMES (remove or fix old frames)
+ if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_+1)
+ {
+ //std::cout << "first_window_frame_ " << (*first_window_frame_)->nodeId() << std::endl;
+ //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
+ (*first_window_frame_)->fix();
+ first_window_frame_++;
+ }
+ std::cout << "window managed" << std::endl;
+ }
+
+ bool check_new_frame(CaptureBase* new_capture)
+ {
+ std::cout << "checking if new frame..." << std::endl;
+ // TODO: not only time, depending on the sensor...
+ std::cout << new_capture->getTimeStamp().get() - last_frame_->getTimeStamp().get() << std::endl;
+ return new_capture->getTimeStamp().get() - last_frame_->getTimeStamp().get() > new_frame_elapsed_time_;
}
void update()
{
+ std::cout << "updating..." << std::endl;
while (!new_captures_.empty())
{
// EXTRACT NEW CAPTURE
CaptureBase* new_capture = new_captures_.front();
new_captures_.pop();
+ // OVERWRITE CURRENT STAMP
+ current_frame_->setTimeStamp(new_capture->getTimeStamp());
+
+ // INITIALIZE FIRST FRAME STAMP
+ if (last_frame_->getTimeStamp().get() == 0)
+ last_frame_->setTimeStamp(new_capture->getTimeStamp());
+
// ODOMETRY SENSOR
if (new_capture->getSensorPtr() == sensor_prior_)
{
- // UPDATE LAST STATE FROM SECOND LAST (optimized) TODO: see if it is necessary
- if (second_last_capture_relative_ != nullptr)
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setState(second_last_capture_relative_->computePrior());
+ std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl;
+ // NEW KEY FRAME ?
+ if (check_new_frame(new_capture))
+ createFrame(new_capture->getTimeStamp());
- // INTEGRATE NEW ODOMETRY TO LAST FRAME
- last_capture_relative_->integrateCapture((CaptureRelative*) (new_capture));
-
- // INITIALIZE STAMP OF FIRST FRAME
- //std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
- if (problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() == 0)
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setTimeStamp(new_capture->getTimeStamp());
-
- // NEW KEY FRAME (if enough time from last frame)
- if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > new_frame_elapsed_time_)
+ // ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
+ if (last_capture_relative_ == nullptr)
{
- //std::cout << "store prev frame" << std::endl;
- auto second_last_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
-
- // NEW CURRENT FRAME
- //std::cout << "new frame" << std::endl;
- createFrame(last_capture_relative_->computePrior(), new_capture->getTimeStamp());
-
- // COMPUTE PREVIOUS FRAME CAPTURES
- //std::cout << "compute prev frame" << std::endl;
- for (auto capture_it = second_last_frame_ptr->getCaptureListPtr()->begin(); capture_it != second_last_frame_ptr->getCaptureListPtr()->end(); capture_it++)
- (*capture_it)->processCapture();
-
- // WINDOW of FRAMES (remove or fix old frames)
- //std::cout << "frame window" << std::endl;
- if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_)
- {
- //std::cout << "frame fixing" << std::endl;
- //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
- (*first_window_frame_)->fix();
- first_window_frame_++;
- }
- else
- first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
+ last_frame_->addCapture(new_capture);
+ new_capture->processCapture();
+ last_capture_relative_ = (CaptureRelative*)new_capture;
}
+ else
+ last_capture_relative_->integrateCapture((CaptureRelative*) (new_capture));
+ current_frame_->setState(last_capture_relative_->computePrior());
+ current_frame_->setTimeStamp(new_capture->getTimeStamp());
}
else
{
- // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
- //std::cout << "adding not odometry capture " << new_capture->nodeId() << std::endl;
- bool same_sensor_capture_found = false;
- for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin();
- capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
+ std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl;
+ // NEW KEY FRAME ?
+ if (check_new_frame(new_capture))
+ createFrame(new_capture->getTimeStamp());
+
+ // ADD CAPTURE TO THE CURRENT FRAME (or substitute the same sensor previous capture)
+ std::cout << "searching repeated capture..." << new_capture->nodeId() << std::endl;
+ CaptureBaseIter repeated_capture_it = current_frame_->hasCaptureOf(new_capture->getSensorPtr());
+
+ if (repeated_capture_it != current_frame_->getCaptureListPtr()->end()) // repeated capture
+ {
+ std::cout << "repeated capture, keeping old capture" << new_capture->nodeId() << std::endl;
+ //current_frame_->removeCapture(repeated_capture_it);
+ }
+ else
{
- if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr())
- {
- //std::cout << "removing previous capture" << std::endl;
- //problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
- same_sensor_capture_found = true;
- //std::cout << "removed!" << std::endl;
- break;
- }
+ std::cout << "not repeated, adding capture..." << new_capture->nodeId() << std::endl;
+ last_frame_->addCapture(new_capture);
+ std::cout << "processing capture..." << new_capture->nodeId() << std::endl;
+ new_capture->processCapture();
+ std::cout << "processed" << std::endl;
}
- if (!same_sensor_capture_found)
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
}
}
+ std::cout << "updated" << std::endl;
}
Eigen::VectorXs getVehiclePose()
{
- if (second_last_capture_relative_ != nullptr)
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setState(second_last_capture_relative_->computePrior());
return last_capture_relative_->computePrior();
}
diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp
index 4a8c0b3c94f1c2773d3b078ddd917d5c862cdf4e..3b794498e5c4a517d90678002f790dc8f36da059 100644
--- a/src/wolf_problem.cpp
+++ b/src/wolf_problem.cpp
@@ -41,13 +41,19 @@ bool WolfProblem::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _ne
// Check if resize should be done
if (state_idx_last_+_new_state_ptr->getStateSize() > state_.size())
{
+ std::cout << "Resizing state and updating asl state units pointers..." << std::endl;
std::cout << "\nState size: " << state_.size() << " last idx: " << state_idx_last_ << " last idx + new state size: " << state_idx_last_+_new_state_ptr->getStateSize() << std::endl;
- std::cout << "Resizing state and remapping al state units..." << std::endl;
- WolfScalar* old_first_pointer = state_.data();
- state_.resize(state_.size()*2);
+ WolfScalar* old_first_pointer = state_.data();
+ state_.conservativeResize(state_.size()*2);
+ covariance_.conservativeResize(state_.size()*2,state_.size()*2);
for (auto state_units_it = state_list_.begin(); state_units_it != state_list_.end(); state_units_it++)
- (*state_units_it)->setPtr(state_.data() + ( (*state_units_it)->getPtr() - old_first_pointer) );
- std::cout << "New state size: " << state_.size() << "last idx: " << state_idx_last_ << std::endl;
+ {
+ //std::cout << "state unit: " << (*state_units_it)->nodeId() << std::endl;
+ (*state_units_it)->setPtr(state_.data() + ( (*state_units_it)->getPtr() - old_first_pointer) );
+ }
+ std::cout << "----------------------------- difference of location: " << old_first_pointer - state_.data() << std::endl;
+ _new_state_ptr->setPtr(state_.data()+state_idx_last_);
+ std::cout << "New state size: " << state_.size() << " last idx: " << state_idx_last_ << std::endl;
reallocated_ = true;
}
//std::cout << "\nnew state unit: " << _new_state_values.transpose() << std::endl;
@@ -69,8 +75,14 @@ bool WolfProblem::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _ne
void WolfProblem::addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov)
{
- assert(_state1 != nullptr && _state1->getPtr() != nullptr && _state1->getPtr() < state_.data() + state_idx_last_ && _state1->getPtr() > state_.data());
- assert(_state2 != nullptr && _state2->getPtr() != nullptr && _state2->getPtr() < state_.data() + state_idx_last_ && _state2->getPtr() > state_.data());
+ assert(_state1 != nullptr);
+ assert(_state1->getPtr() != nullptr);
+ assert(_state1->getPtr() < state_.data() + state_idx_last_);
+ assert(_state1->getPtr() > state_.data());
+ assert(_state2 != nullptr);
+ assert(_state2->getPtr() != nullptr);
+ assert(_state2->getPtr() < state_.data() + state_idx_last_);
+ assert(_state2->getPtr() > state_.data());
// Guarantee that we are updating the top triangular matrix (in cross covariance case)
bool flip = _state1->getPtr() > _state2->getPtr();
@@ -91,8 +103,14 @@ void WolfProblem::addCovarianceBlock(StateBase* _state1, StateBase* _state2, con
void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const
{
- assert(_state1 != nullptr && _state1->getPtr() != nullptr && _state1->getPtr() < state_.data() + state_idx_last_ && _state1->getPtr() > state_.data());
- assert(_state2 != nullptr && _state2->getPtr() != nullptr && _state2->getPtr() < state_.data() + state_idx_last_ && _state2->getPtr() > state_.data());
+ assert(_state1 != nullptr);
+ assert(_state1->getPtr() != nullptr);
+ assert(_state1->getPtr() < state_.data() + state_idx_last_);
+ assert(_state1->getPtr() > state_.data());
+ assert(_state2 != nullptr);
+ assert(_state2->getPtr() != nullptr);
+ assert(_state2->getPtr() < state_.data() + state_idx_last_);
+ assert(_state2->getPtr() > state_.data());
// Guarantee that we are getting the top triangular matrix (in cross covariance case)
bool flip = _state1->getPtr() > _state2->getPtr();
@@ -110,8 +128,14 @@ void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eig
void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::Map<Eigen::MatrixXs>& _cov_block) const
{
- assert(_state1 != nullptr && _state1->getPtr() != nullptr && _state1->getPtr() < state_.data() + state_idx_last_ && _state1->getPtr() > state_.data());
- assert(_state2 != nullptr && _state2->getPtr() != nullptr && _state2->getPtr() < state_.data() + state_idx_last_ && _state2->getPtr() > state_.data());
+ assert(_state1 != nullptr);
+ assert(_state1->getPtr() != nullptr);
+ assert(_state1->getPtr() < state_.data() + state_idx_last_);
+ assert(_state1->getPtr() > state_.data());
+ assert(_state2 != nullptr);
+ assert(_state2->getPtr() != nullptr);
+ assert(_state2->getPtr() < state_.data() + state_idx_last_);
+ assert(_state2->getPtr() > state_.data());
// Guarantee that we are getting the top triangular matrix (in cross covariance case)
bool flip = _state1->getPtr() > _state2->getPtr();
@@ -172,6 +196,11 @@ TrajectoryBase* WolfProblem::getTrajectoryPtr()
return trajectory_ptr_;
}
+FrameBase* WolfProblem::getLastFramePtr()
+{
+ return trajectory_ptr_->getLastFramePtr();
+}
+
StateBaseList* WolfProblem::getStateListPtr()
{
return &state_list_;
diff --git a/src/wolf_problem.h b/src/wolf_problem.h
index 62c5cfd7cc31db6be45f9d21e2db4468994269cc..e8bbe5f8c3df416a474a57e95d1f28f217bc4c43 100644
--- a/src/wolf_problem.h
+++ b/src/wolf_problem.h
@@ -153,6 +153,13 @@ class WolfProblem: public NodeBase
*/
TrajectoryBase* getTrajectoryPtr();
+ /** \brief Returns a pointer to last Frame
+ *
+ * Returns a pointer to last Frame
+ *
+ **/
+ FrameBase* getLastFramePtr();
+
/** \brief Gets a pointer to the state units list
*
* Gets a pointer to the state units list