Skip to content
Snippets Groups Projects
Commit 06a96398 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix some bugs related to orig/last pointers

parent 1f32acd8
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
......@@ -88,6 +88,7 @@ unsigned int ProcessorOdomIcp3d::processKnown()
{
if (origin_ptr_ && (incoming_ptr_ != origin_ptr_))
{
origin_laser_ = std::static_pointer_cast<CaptureLaser3d>(origin_ptr_);
pairAlign(origin_laser_->getPointCloud(),
incoming_laser_->getPointCloud(),
T_origin_last_,
......@@ -102,11 +103,15 @@ unsigned int ProcessorOdomIcp3d::processKnown()
*/
unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
{
pairAlign(last_laser_->getPointCloud(),
incoming_laser_->getPointCloud(),
Eigen::Isometry3d::Identity(),
T_last_incoming_,
registration_solver_);
if (last_ptr_)
{
last_laser_ = std::static_pointer_cast<CaptureLaser3d>(last_ptr_);
pairAlign(last_laser_->getPointCloud(),
incoming_laser_->getPointCloud(),
Eigen::Isometry3d::Identity(),
T_last_incoming_,
registration_solver_);
}
return 0;
};
......@@ -123,6 +128,10 @@ bool ProcessorOdomIcp3d::voteForKeyFrame() const
{
return true;
}
// TODO:
// - vote by distance
// - vote by angle
// - vote by nbr. of captures
return false;
};
......@@ -156,13 +165,13 @@ TimeStamp ProcessorOdomIcp3d::getTimeStamp() const
VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const
{
VectorComposite state_origin = origin_laser_->getFrame()->getState("PO");
VectorComposite state_origin = origin_ptr_->getFrame()->getState("PO");
Eigen::Isometry3d T_world_origin_robot =
Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data());
Eigen::Isometry3d T_world_last_robot = T_world_origin_robot * T_robot_sensor_ * T_origin_last_ * T_sensor_robot_;
VectorComposite state_last;
state_last.at('P') = T_world_last_robot.translation();
state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
state_last['P'] = T_world_last_robot.translation();
state_last['O'] = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
return state_last;
}
......@@ -177,16 +186,19 @@ VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateSt
*/
void ProcessorOdomIcp3d::establishFactors()
{
// emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
Eigen::Vector7d measurement_of_motion;
measurement_of_motion.head(3) = T_origin_last_.translation();
measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs();
auto feature =
FeatureBase::emplace<FeatureBase>(last_laser_, "FeatureBase", measurement_of_motion, transform_cov_);
// emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION);
if (last_ptr_ != origin_ptr_) // skip if it's the same frame (it happens the SECOND_TIME)
{
// emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
Eigen::Vector7d measurement_of_motion;
measurement_of_motion.head(3) = T_origin_last_.translation();
measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs();
auto feature =
FeatureBase::emplace<FeatureBase>(last_ptr_, "FeatureBase", measurement_of_motion, transform_cov_);
// emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
feature, feature, origin_ptr_->getFrame(), shared_from_this(), false, TOP_MOTION);
}
};
} // namespace wolf
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment