From 13a9c2e67197a65fa51f1a035953e16b0e1d3894 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 17 Dec 2015 11:02:31 +0100
Subject: [PATCH] autodiffcostfunctionwrapper and all changes related. Fixed
duplicated data_ and data_covariance_ both in captureMotion and CaptureOdom2D
---
src/CMakeLists.txt | 1 +
src/capture_fix.cpp | 2 +-
src/capture_odom_2D.h | 5 +-
.../auto_diff_cost_function_wrapper.h | 150 ++++++++----------
src/ceres_wrapper/ceres_manager.cpp | 3 +-
src/constraint_container.h | 2 -
src/constraint_corner_2D.h | 3 +-
src/constraint_fix.h | 4 +
src/constraint_odom_2D.h | 4 +
src/examples/CMakeLists.txt | 5 +
src/examples/solver/test_iQR_wolf2.cpp | 21 +--
src/examples/test_ceres_2_lasers.cpp | 21 +--
src/feature_odom_2D.cpp | 2 +-
src/processor_laser_2D.cpp | 5 +-
src/solver/qr_solver.h | 8 +-
src/wolf_manager.cpp | 2 -
16 files changed, 123 insertions(+), 115 deletions(-)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 2a49b96da..b7ddaffe9 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -92,6 +92,7 @@ SET(HDRS
node_linked.h
processor_base.h
processor_laser_2D.h
+ raw_data_satellite.h
sensor_base.h
sensor_odom_2D.h
sensor_gps_fix.h
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index d665f7aeb..5a413a1e2 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -6,7 +6,7 @@ CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eige
data_(_data),
data_covariance_(_data_covariance)
{
- std::cout << "capture fix constructor " << std::endl;
+ //std::cout << "capture fix constructor " << std::endl;
}
CaptureFix::~CaptureFix()
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index ad535c270..b8c996418 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -15,9 +15,8 @@ class CaptureOdom2D : public CaptureMotion
{
protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
-
+ //Eigen::VectorXs data_; ///< Raw data.
+ //Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
public:
diff --git a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
index 978da76a8..f948cf58e 100644
--- a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
+++ b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
@@ -55,7 +55,7 @@ class AutoDiffCostFunctionWrapper : public ceres::SizedCostFunction<MEASUREMENT_
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
{
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
parameters[5], parameters[6], parameters[7], parameters[8], parameters[9], residuals);
@@ -107,13 +107,12 @@ class AutoDiffCostFunctionWrapper : public ceres::SizedCostFunction<MEASUREMENT_
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<this->n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<this->n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -169,7 +168,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
parameters[5], parameters[6], parameters[7], parameters[8], residuals);
@@ -217,13 +216,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -278,7 +276,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
parameters[5], parameters[6], parameters[7], residuals);
@@ -323,13 +321,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -383,7 +380,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
parameters[5], parameters[6], residuals);
@@ -425,13 +422,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -484,7 +480,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
parameters[5], residuals);
@@ -523,13 +519,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -578,7 +573,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4],
residuals);
@@ -614,13 +609,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -668,7 +662,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], residuals);
// also compute jacobians
else
@@ -699,13 +693,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -752,7 +745,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], residuals);
// also compute jacobians
@@ -781,13 +774,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -833,7 +825,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], parameters[1], residuals);
// also compute jacobians
@@ -859,13 +851,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
@@ -910,7 +901,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// only residuals
- if (jacobians != nullptr)
+ if (jacobians == nullptr)
(*this->constraint_ptr_)(parameters[0], residuals);
// also compute jacobians
else
@@ -932,13 +923,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE,
residuals[i] = residuals_jets[i].a;
// fill the jacobian matrices
- if (jacobians != nullptr)
- for (i = 0; i<n_blocks_; i++)
- if (jacobians[i] != nullptr)
- for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
- std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
- residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
- jacobians[i] + row * block_sizes_.at(i));
+ for (i = 0; i<n_blocks_; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++)
+ std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i),
+ jacobians[i] + row * block_sizes_.at(i));
}
return true;
}
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 75bd626e9..889361768 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -37,7 +37,6 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
// run Ceres Solver
ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
- //std::cout << "solved" << std::endl;
//return results
return ceres_summary_;
@@ -224,7 +223,7 @@ void CeresManager::removeConstraint(const unsigned int& _corr_id)
void CeresManager::addStateBlock(StateBlock* _st_ptr)
{
- //std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl;
+ //std::cout << "Adding State Unit with size: " << _st_ptr->getSize() << std::endl;
//_st_ptr->print();
switch (_st_ptr->getType())
diff --git a/src/constraint_container.h b/src/constraint_container.h
index dd45e22fc..53deffda0 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -21,7 +21,6 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
corner_(_corner)
{
assert(_corner >= 0 && _corner <= 3 && "Wrong corner id in constraint container constructor");
- lmk_ptr_->addConstraintTo(this);
}
/** \brief Default destructor (not recommended)
@@ -32,7 +31,6 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
virtual ~ConstraintContainer()
{
//std::cout << "deleting ConstraintContainer " << nodeId() << std::endl;
- lmk_ptr_->removeConstraintTo(this);
}
LandmarkContainer* getLandmarkPtr()
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index d603f9165..e74217a38 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -14,7 +14,7 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<3,2,1,2,1>(_ftr_ptr, CTR_CORNER_2D, _lmk_ptr, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
{
- landmark_ptr_->addConstraintTo(this);
+ //
}
/** \brief Default destructor (not recommended)
@@ -25,7 +25,6 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
virtual ~ConstraintCorner2D()
{
//std::cout << "deleting ConstraintCorner2D " << nodeId() << std::endl;
- landmark_ptr_->removeConstraintTo(this);
}
LandmarkCorner2D* getLandmarkPtr()
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index fef49f4b9..6258758b8 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -30,6 +30,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
template <typename T>
bool operator()(const T* const _p, const T* const _o, T* _residuals) const
{
+ //std::cout << "computing constraint odom ..." << std::endl;
+
_residuals[0] = (T(getMeasurement()(0)) - _p[0]) / T(sqrt(getMeasurementCovariance()(0,0)));
_residuals[1] = (T(getMeasurement()(1)) - _p[1]) / T(sqrt(getMeasurementCovariance()(1,1)));
_residuals[2] = T(getMeasurement()(2)) - _o[0];
@@ -49,6 +51,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
// std::cout << "residual: " << _residuals[2] << std::endl << std::endl;
_residuals[2] = _residuals[2] / T(sqrt(getMeasurementCovariance()(2,2)));
+ //std::cout << "constraint fix computed!" << std::endl;
+
return true;
}
};
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 312d55c3e..83c9706ba 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -29,6 +29,8 @@ class ConstraintOdom2D : 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 << "computing constraint odom ..." << std::endl;
+
// std::cout << "_p1: ";
// for (int i=0; i < 2; i++)
// std::cout << "\n\t" << _p1[i];
@@ -72,6 +74,8 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
_residuals[2] = _residuals[2] / T(sqrt(std::max(getMeasurementCovariance()(2, 2),1e-6)));
+ //std::cout << "constraint odom computed!" << std::endl;
+
return true;
}
};
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index c637f88d7..697310286 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -52,6 +52,11 @@ IF(faramotics_FOUND)
${pose_state_time_LIBRARIES}
${faramotics_LIBRARIES}
${PROJECT_NAME})
+ ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
+ TARGET_LINK_LIBRARIES(test_autodiff
+ ${pose_state_time_LIBRARIES}
+ ${faramotics_LIBRARIES}
+ ${PROJECT_NAME})
IF(Suitesparse_FOUND)
ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index a75890a5a..9250a851c 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -19,6 +19,7 @@
//Wolf includes
#include "state_block.h"
#include "constraint_base.h"
+#include "sensor_laser_2D.h"
#include "wolf_manager.h"
// wolf solver
@@ -281,16 +282,16 @@ int main(int argc, char *argv[])
// DRAWING STUFF ---------------------------
// draw detected corners
- std::list < laserscanutils::Corner > corner_list;
- std::vector<double> corner_vector;
- CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
- last_scan.extractCorners(corner_list);
- for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
- {
- corner_vector.push_back(corner_it->pt_(0));
- corner_vector.push_back(corner_it->pt_(1));
- }
- myRender->drawCorners(laser1Pose, corner_vector);
+// std::list < laserscanutils::Corner > corner_list;
+// std::vector<double> corner_vector;
+// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+// last_scan.extractCorners(corner_list);
+// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
+// {
+// corner_vector.push_back(corner_it->pt_(0));
+// corner_vector.push_back(corner_it->pt_(1));
+// }
+// myRender->drawCorners(laser1Pose, corner_vector);
// draw landmarks
std::vector<double> landmark_vector;
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index c3ac998f6..e3431fd54 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -17,6 +17,7 @@
//Wolf includes
#include "wolf_manager.h"
+#include "sensor_laser_2D.h"
#include "ceres_wrapper/ceres_manager.h"
//C includes for sleep, time and main args
@@ -228,16 +229,16 @@ int main(int argc, char** argv)
// DRAWING STUFF ---------------------------
t1 = clock();
// draw detected corners
- std::list < laserscanutils::Corner > corner_list;
- std::vector<double> corner_vector;
- CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
- last_scan.extractCorners(corner_list);
- for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
- {
- corner_vector.push_back(corner_it->pt_(0));
- corner_vector.push_back(corner_it->pt_(1));
- }
- myRender->drawCorners(laser1Pose, corner_vector);
+// std::list < laserscanutils::Corner > corner_list;
+// std::vector<double> corner_vector;
+// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+// last_scan.extractCorners(corner_list);
+// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
+// {
+// corner_vector.push_back(corner_it->pt_(0));
+// corner_vector.push_back(corner_it->pt_(1));
+// }
+// myRender->drawCorners(laser1Pose, corner_vector);
// draw landmarks
std::vector<double> landmark_vector;
diff --git a/src/feature_odom_2D.cpp b/src/feature_odom_2D.cpp
index 4c60e41ef..0eb5be107 100644
--- a/src/feature_odom_2D.cpp
+++ b/src/feature_odom_2D.cpp
@@ -9,7 +9,7 @@ FeatureOdom2D::FeatureOdom2D(unsigned int _dim_measurement) :
FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
FeatureBase(_measurement, _meas_covariance)
{
- //
+ //std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
}
FeatureOdom2D::~FeatureOdom2D()
diff --git a/src/processor_laser_2D.cpp b/src/processor_laser_2D.cpp
index 0b05a15ec..4f24b920c 100644
--- a/src/processor_laser_2D.cpp
+++ b/src/processor_laser_2D.cpp
@@ -2,7 +2,7 @@
ProcessorLaser2D::ProcessorLaser2D() :
- sensor_laser_ptr_((SensorLaser2D*)(upperNodePtr())), // Static cast to specific sensor at construction time
+ //sensor_laser_ptr_((SensorLaser2D*)(upperNodePtr())), // Static cast to specific sensor at construction time TODO: in construction time upperNodePtr is nullptr, it crashes always, to be removed or changed to somewhere (JVN)
capture_laser_ptr_(nullptr)
{
}
@@ -13,6 +13,9 @@ ProcessorLaser2D::~ProcessorLaser2D()
void ProcessorLaser2D::extractFeatures(CaptureBase* capture_ptr_)
{
+ // TODO: Or we always cast (so pointer is not needed) or we find the place to be casted once, but constructor is not the place.
+ sensor_laser_ptr_ = (SensorLaser2D*)(upperNodePtr());
+
//check CaptureBase pointer is the appropriate one for this Processor
// assert( capture_laser_ptr_ = dynamic_cast<CaptureLaser2D*>(capture_ptr_) && "Invalid Capture Type pointer" );
capture_laser_ptr_ = (CaptureLaser2D*)(capture_ptr_);
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index e1f86e5dd..b8991339c 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -14,7 +14,13 @@
//Wolf includes
#include "state_block.h"
-#include "constraint_base.h"
+#include "../constraint_sparse.h"
+#include "../constraint_fix.h"
+#include "../constraint_gps_2D.h"
+#include "../constraint_gps_pseudorange.h"
+#include "../constraint_odom_2D.h"
+#include "../constraint_corner_2D.h"
+#include "../constraint_container.h"
#include "sparse_utils.h"
// wolf solver
diff --git a/src/wolf_manager.cpp b/src/wolf_manager.cpp
index 5fa139d08..2184fce63 100644
--- a/src/wolf_manager.cpp
+++ b/src/wolf_manager.cpp
@@ -40,8 +40,6 @@ WolfManager::WolfManager(const FrameStructure _frame_structure,
//std::cout << " initial_covariance" << std::endl;
current_frame_->addCapture(initial_covariance);
//std::cout << " addCapture" << std::endl;
- initial_covariance->process();
- //std::cout << " processCapture" << std::endl;
// Current robot frame
createFrame(_prior, TimeStamp(0));
--
GitLab