Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/plugins/laser
1 result
Show changes
Commits on Source (10)
Hide whitespace changes
Inline Side-by-side
Showing
with 150 additions and 140 deletions
CMakeLists.txt
View file @ 03182efe
......@@ -146,6 +146,7 @@ INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
SET(HDRS_COMMON
)
SET(HDRS_MATH
include/laser/math/laser_tools.h
)
SET(HDRS_UTILS
)
......
demos/demo_2_lasers_offline.cpp
View file @ 03182efe
......@@ -171,15 +171,14 @@ int main(int argc, char** argv)
problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts, 0.1);
// Ceres wrapper
ceres::Solver::Options ceres_options;
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
SolverCeres ceres_manager(&problem);
ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3;
// ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false;
// ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS;
// ceres_manager.getSolverOptions().max_num_iterations = 100;
google::InitGoogleLogging(argv[0]);
CeresManager ceres_manager(&problem, ceres_options);
std::ofstream log_file, landmark_file; //output file
std::cout << "START TRAJECTORY..." << std::endl;
......
demos/demo_ceres_2_lasers.cpp
View file @ 03182efe
......@@ -257,15 +257,15 @@ int main(int argc, char** argv)
odom_processor->setOrigin(origin_frame);
// Ceres wrapper
ceres::Solver::Options ceres_options;
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
SolverCeres ceres_manager(&problem);
ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3;
// ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false;
// ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS;
// ceres_manager.getSolverOptions().max_num_iterations = 100;
google::InitGoogleLogging(argv[0]);
CeresManager ceres_manager(&problem, ceres_options);
SolverCeres ceres_manager(&problem, ceres_options);
std::ofstream log_file, landmark_file; //output file
//std::cout << "START TRAJECTORY..." << std::endl;
......
demos/demo_ceres_2_lasers_polylines.cpp
View file @ 03182efe
......@@ -264,15 +264,15 @@ int main(int argc, char** argv)
odom_processor->setOrigin(origin_frame);
// Ceres wrapper
ceres::Solver::Options ceres_options;
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
SolverCeres ceres_manager(&problem);
ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3;
// ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false;
// ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS;
// ceres_manager.getSolverOptions().max_num_iterations = 100;
google::InitGoogleLogging(argv[0]);
CeresManager ceres_manager(&problem, ceres_options);
SolverCeres ceres_manager(&problem, ceres_options);
std::ofstream log_file, landmark_file; //output file
//std::cout << "START TRAJECTORY..." << std::endl;
......
include/laser/math/laser_tools.h 0 → 100644
View file @ 03182efe
/*
* laser_tools.h
*
* Created on: Jun 17, 2020
* Author: jsola
*/
#ifndef INCLUDE_LASER_MATH_LASER_TOOLS_H_
#define INCLUDE_LASER_MATH_LASER_TOOLS_H_
#include <core/common/wolf.h>
#include <core/state_block/state_composite.h>
#include <laser_scan_utils/laser_scan_utils.h>
#include <laser_scan_utils/icp.h>
using namespace Eigen;
namespace wolf
{
namespace laser
{
inline Eigen::Isometry2d trf2isometry(Vector2d translation, Vector1d rotation)
{
Isometry2d T = Translation2d(translation) * Rotation2Dd(rotation(0));
return T;
}
inline Eigen::Isometry2d trf2isometry(Vector3d trf)
{
Isometry2d T = Translation2d(trf.head<2>()) * Rotation2Dd(trf(2));
return T;
}
inline Eigen::Isometry2d trf2isometry(VectorComposite trf)
{
return trf2isometry(trf.at('P'), trf.at('O'));
}
} // namespace laser
} // namespace wolf
#endif /* INCLUDE_LASER_MATH_LASER_TOOLS_H_ */
include/laser/processor/processor_odom_icp.h
View file @ 03182efe
......@@ -169,9 +169,9 @@ class ProcessorOdomIcp : public ProcessorTracker, public IsMotion
~ProcessorOdomIcp() override;
void configure(SensorBasePtr _sensor) override;
VectorComposite getState() const override;
VectorComposite getState(const StateStructure& _structure = "") const override;
TimeStamp getTimeStamp( ) const override;
VectorComposite getState(const TimeStamp& _ts) const override;
VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
void setProblem(ProblemPtr _problem_ptr) override;
protected:
......
src/processor/processor_odom_icp.cpp
View file @ 03182efe
#include "laser/processor/processor_odom_icp.h"
#include "core/common/wolf.h"
#include "laser/math/laser_tools.h"
#include <core/common/wolf.h>
using namespace wolf;
using namespace laserscanutils;
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params)
ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params),
IsMotion("PO")
{
proc_params_ = _params;
......@@ -53,7 +56,6 @@ ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
trf_origin_last_.res_covar = Eigen::Matrix3d::Identity();
trf_origin_incoming_.res_covar = Eigen::Matrix3d::Identity();
trf_last_incoming_.res_covar = Eigen::Matrix3d::Identity();
setStateStructure("PO");
}
ProcessorOdomIcp::~ProcessorOdomIcp()
......@@ -84,46 +86,12 @@ unsigned int ProcessorOdomIcp::processKnown()
CaptureLaser2dPtr origin_ptr = std::static_pointer_cast<CaptureLaser2d>(origin_ptr_);
CaptureLaser2dPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_);
// WOLF_INFO("max_correspondence_dist ", this->icp_params_.max_correspondence_dist);
// WOLF_INFO("max_iterations ", this->icp_params_.max_iterations);
// WOLF_INFO("outliers_adaptive_mult ", this->icp_params_.outliers_adaptive_mult);
// WOLF_INFO("outliers_adaptive_order ", this->icp_params_.outliers_adaptive_order);
// WOLF_INFO("outliers_maxPerc ", this->icp_params_.outliers_maxPerc);
// WOLF_INFO("use_corr_tricks ", this->icp_params_.use_corr_tricks);
// WOLF_INFO("use_point_to_line_distance ", this->icp_params_.use_point_to_line_distance);
// WOLF_INFO("angle_max_ ", this->laser_scan_params_.angle_max_);
// WOLF_INFO("angle_min_ ", this->laser_scan_params_.angle_min_);
// WOLF_INFO("angle_std_dev_ ", this->laser_scan_params_.angle_std_dev_);
// WOLF_INFO("angle_step_ ", this->laser_scan_params_.angle_step_);
// WOLF_INFO("range_max_ ", this->laser_scan_params_.range_max_);
// WOLF_INFO("range_min_ ", this->laser_scan_params_.range_min_);
// WOLF_INFO("range_std_dev_ ", this->laser_scan_params_.range_std_dev_);
// WOLF_INFO("scan_time_ ", this->laser_scan_params_.scan_time_);
// WOLF_INFO("ODOM Icp: Aligning scans ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
// WOLF_INFO("Laser scan params\n");
// this->laser_scan_params_.print();
// WOLF_INFO("Icp params\n");
// WOLF_INFO("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
// "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance, "\n max_iterations: ", this->icp_params_.max_iterations,
// "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
// "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order, "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc,
// "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
origin_ptr->getScan(),
this->laser_scan_params_,
this->icp_params_,
this->trf_origin_last_.res_transf); // Check order
// double score_normalized
// ((double)trf_origin_incoming_.nvalid / (double)min(incoming_ptr->getScan().ranges_raw_.size(), origin_ptr->getScan().ranges_raw_.size()));
// double mean_error = trf_origin_incoming_.error / trf_origin_incoming_.nvalid;
// WOLF_INFO("DBG ------------------------------");
// WOLF_INFO("DBG valid? ", trf_origin_incoming_.valid, " m_er ", mean_error, " ", score_normalized * 100, "% own_id: ", incoming_ptr->id(),
// " other_id: ", origin_ptr->id());
// WOLF_INFO("DBG own_POSE: 0 0 0 other_POSE: ", this->trf_origin_last_.res_transf.transpose(),
// " Icp_guess: ", this->trf_origin_last_.res_transf.transpose(), " Icp_trf_origin_incoming_: ", trf_origin_incoming_.res_transf.transpose());
// WOLF_INFO("DBG odometry");
trf_origin_incoming_.valid = trf_origin_incoming_.valid && trf_origin_incoming_.error / trf_origin_incoming_.nvalid < 5e-2;
}
return 0;
......@@ -134,7 +102,6 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
Eigen::Vector3d t_identity;
t_identity << 0, 0, 0;
// XXX: Dynamic or static? JS: static is OK.
CaptureLaser2dPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_);
CaptureLaser2dPtr last_ptr = std::static_pointer_cast<CaptureLaser2d>(last_ptr_);
......@@ -144,28 +111,8 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
this->icp_params_,
t_identity);
// WOLF_INFO("ODOM Icp: Aligning scans ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
// WOLF_INFO("Laser scan params\n");
// this->laser_scan_params_.print();
// WOLF_INFO("Icp params\n");
// WOLF_INFO("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist, "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance, "\n max_iterations: ", this->icp_params_.max_iterations,
// "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult, "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order, "\n outliers_maxPerc: ",
// this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
trf_last_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(), last_ptr->getScan(), this->laser_scan_params_, this->icp_params_, t_identity);
//TEST! TO BE REMOVED
// trf_last_incoming_.res_transf = t_identity;
// WOLF_TRACE("trf_last_incoming_: ", trf_last_incoming_.res_transf.transpose());
// Eigen::Matrix2s R_last;
// double alphal = trf_origin_last_.res_transf(2);
// double alphali = trf_origin_incoming_.res_transf(2) - alphal;
// R_last << cos(alphal), -sin(alphal), sin(alphal), cos(alphal);
// Eigen::Vector3s trf_last_incoming_check;
// trf_last_incoming_check.head(2) = R_last.transpose() * (trf_origin_incoming_.res_transf.head(2) - trf_origin_last_.res_transf.head(2));
// trf_last_incoming_check(2) = alphali;
// WOLF_TRACE("trf_last_incoming_check: ", trf_last_incoming_check.transpose());
trf_last_incoming_.valid = trf_last_incoming_.valid && trf_last_incoming_.error / trf_last_incoming_.nvalid < 5e-2;
return 0;
......@@ -236,60 +183,69 @@ void ProcessorOdomIcp::advanceDerived()
// overwrite last frame's state
Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().at("P")) * Rotation2Dd(origin_ptr_->getFrame()->getState().at("O")(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
const auto& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
Isometry2d so_T_si = Translation2d(trf_origin_incoming_.res_transf.head(2)) * Rotation2Dd(trf_origin_incoming_.res_transf(2));
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_inc; x_inc << w_T_ri.translation() , Rotation2Dd(w_T_ri.rotation()).angle();
// Put the state of incoming in last
last_ptr_->getFrame()->setState(x_inc, "PO", {2,1});
// computing odometry
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
odometry_['P'] = sl_T_si.translation();
odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
// advance transforms
trf_origin_last_ = trf_origin_incoming_;
}
void ProcessorOdomIcp::resetDerived()
{
// WOLF_INFO("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
if (origin_ptr_ != nullptr and last_ptr_ != nullptr)
{
// std::cout << "NDEBUG origin " << std::setw(4) << origin_ptr_->id()
// << " last " << std::setw(4) << last_ptr_->id()
// << " is valid? " << std::setw(4) << trf_origin_last_.valid
// << " error " << std::setw(8) << trf_origin_last_.error
// << " merror " << std::setw(10) << trf_origin_last_.error / (double)trf_origin_last_.nvalid
// << " nvalid " << std::setw(3) << trf_origin_last_.nvalid
// << " sqrt diag. " << std::setw(10) << trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose()
// << " trnsf " << std::setw(10) << trf_origin_last_.res_transf.transpose() << std::endl;
}
// Using processing_step_ to ensure that origin, last and incoming are different
if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) {
// Notation explanation:
// x1_R_x2: Rotation from frame x1 to frame x2
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
Isometry2d so_T_si = Translation2d(trf_origin_incoming_.res_transf.head(2)) * Rotation2Dd(trf_origin_incoming_.res_transf(2));
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_current;
x_current << w_T_ri.translation(), Rotation2Dd(w_T_ri.rotation()).angle();
// last
last_ptr_->getFrame()->setState(x_current, "PO", {2,1});
// incoming_ptr_->getFrame()->setState(x_current);
trf_origin_last_ = trf_last_incoming_;
// WOLF_INFO("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
if (origin_ptr_ != nullptr and last_ptr_ != nullptr)
{
//
}
// Using processing_step_ to ensure that origin, last and incoming are different
if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) {
// Notation explanation:
// x1_R_x2: Rotation from frame x1 to frame x2
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
const auto &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_current;
x_current << w_T_ri.translation(), Rotation2Dd(w_T_ri.rotation()).angle();
// last
last_ptr_->getFrame()->setState(x_current, "PO", {2,1});
// incoming_ptr_->getFrame()->setState(x_current);
// computing odometry
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
odometry_['P'] = sl_T_si.translation();
odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
// advance transforms
trf_origin_last_ = trf_last_incoming_;
}
}
......@@ -319,23 +275,29 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
params_->apply_loss_function);
}
VectorComposite ProcessorOdomIcp::getState( ) const
VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const
{
Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
if (origin_ptr_ == nullptr or
origin_ptr_->isRemoving() or
last_ptr_ == nullptr or
last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
// Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
{
WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
return VectorComposite(); // return empty state
}
auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
const auto& rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
Isometry2d so_T_si = Translation2d(trf_origin_last_.res_transf.head(2)) * Rotation2Dd(trf_origin_last_.res_transf(2));
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d w_T_rl = w_T_ro * ro_T_so * so_T_sl * rl_T_sl.inverse();
Vector3d x_current;
VectorComposite state("PO", {w_T_ri.translation(), Vector1d(Rotation2Dd(w_T_ri.rotation()).angle())});
// Eigen::Vector3d state = Vector3d(0, 0, 0);
// state.head(2) << w_T_ri.translation();
// state(2) = Rotation2Dd(w_T_ri.rotation()).angle();
VectorComposite state("PO", {w_T_rl.translation(), Vector1d(Rotation2Dd(w_T_rl.rotation()).angle())});
return state;
}
......@@ -343,12 +305,12 @@ VectorComposite ProcessorOdomIcp::getState( ) const
TimeStamp ProcessorOdomIcp::getTimeStamp() const
{
if( last_ptr_ == nullptr )
return 0;
return TimeStamp::Invalid();
else
return last_ptr_->getTimeStamp();
}
VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts) const
VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStructure& _structure) const
{
// todo fix this code to get any state in the whole trajectory!
......
src/processor/processor_tracker_feature_polyline_2d.cpp
View file @ 03182efe
......@@ -1310,8 +1310,8 @@ void ProcessorTrackerFeaturePolyline2d::computeTransformations()
Eigen::Matrix2d R_world_robot_last = Eigen::Rotation2Dd(vehicle_pose_last(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
if (getSensor()->isStateBlockDynamic("P") ||
getSensor()->isStateBlockDynamic("O") ||
if (getSensor()->isStateBlockDynamic('P') ||
getSensor()->isStateBlockDynamic('O') ||
!getSensor()->getP()->isFixed() ||
!getSensor()->getO()->isFixed() ||
!extrinsics_transformation_computed_)
......
test/gtest_processor_odom_icp.cpp
View file @ 03182efe
......@@ -8,7 +8,7 @@
#include "laser/internal/config.h"
#include <core/ceres_wrapper/ceres_manager.h>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "laser/processor/processor_odom_icp.h" // THIS AT THE END OTHERWISE IT FAILS COMPILING
......@@ -37,7 +37,7 @@ class ProcessorOdomIcp_Test : public testing::Test
{
public:
ProblemPtr problem;
CeresManagerPtr solver;
SolverCeresPtr solver;
SensorLaser2dPtr lidar;
ProcessorOdomIcpPublicPtr processor;
......@@ -51,7 +51,7 @@ class ProcessorOdomIcp_Test : public testing::Test
void SetUp() override
{
problem = Problem::create("PO", 2);
solver = std::make_shared<CeresManager>(problem);
solver = std::make_shared<SolverCeres>(problem);
auto sen = problem->installSensor("SensorLaser2d", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2d.yaml");
lidar = std::static_pointer_cast<SensorLaser2d>(sen);
......@@ -161,7 +161,6 @@ TEST_F(ProcessorOdomIcp_Test, solve)
for (auto F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
// F->setState(0.5 * Vector3d::Random());
F->perturb(0.5);
}
......