Commit 089053ea authored by James Ward's avatar James Ward
Browse files

Send result of optimisation to RViz panel

parent 9eee03e0
......@@ -7,6 +7,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
find_package(catkin REQUIRED COMPONENTS
cv_bridge
dynamic_reconfigure
eigen_conversions
image_transport
geometry_msgs
message_generation
......
......@@ -84,7 +84,7 @@ public:
Optimiser(const initial_parameters_t& params);
~Optimiser() = default;
bool optimise();
RotationTranslation optimise();
std::vector<OptimisationSample> samples_;
// Rotation only
......
......@@ -9,6 +9,7 @@
<buildtool_depend>catkin</buildtool_depend>
<depend>cv_bridge</depend>
<depend>eigen_conversions</depend>
<depend>image_transport</depend>
<depend>libnlopt-dev</depend>
<depend>libpcl-all-dev</depend>
......
#include <QGroupBox>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QGroupBox>
#include "cam_lidar_panel.h"
#include <cam_lidar_calibration/Optimise.h>
......@@ -9,7 +12,8 @@ CamLidarPanel::CamLidarPanel(QWidget* parent) : rviz::Panel(parent)
{
optimise_client_ = nh_.serviceClient<cam_lidar_calibration::Optimise>("optimiser");
QVBoxLayout* button_layout = new QVBoxLayout;
QVBoxLayout* main_layout = new QVBoxLayout;
QHBoxLayout* button_layout = new QHBoxLayout;
capture_button_ = new QPushButton("Capture sample");
connect(capture_button_, SIGNAL(clicked()), this, SLOT(captureSample()));
discard_button_ = new QPushButton("Discard last sample");
......@@ -18,11 +22,17 @@ CamLidarPanel::CamLidarPanel(QWidget* parent) : rviz::Panel(parent)
optimise_button_->setEnabled(false);
connect(optimise_button_, SIGNAL(clicked()), this, SLOT(optimise()));
output_label_ = new QLabel("");
button_layout->addWidget(capture_button_);
button_layout->addWidget(discard_button_);
button_layout->addWidget(optimise_button_);
auto button_group = new QGroupBox();
button_group->setLayout(button_layout);
main_layout->addWidget(button_group);
main_layout->addWidget(optimise_button_);
main_layout->addWidget(output_label_);
setLayout(button_layout);
setLayout(main_layout);
}
void CamLidarPanel::captureSample()
......@@ -60,6 +70,13 @@ void CamLidarPanel::optimise()
Optimise srv;
srv.request.operation = Optimise::Request::OPTIMISE;
optimise_client_.call(srv);
auto t = srv.response.transform.translation;
auto r = srv.response.transform.rotation;
std::ostringstream os;
os.precision(3);
os << "Rotation - w: " << r.w << " x: " << r.x << " y: " << r.y << " z: " << r.z;
os << "\nTranslation - x: " << t.x << " y: " << t.y << " z: " << t.z;
output_label_->setText(QString::fromStdString(os.str()));
}
// Save all configuration data from this panel to the given
......
......@@ -7,6 +7,7 @@
#include <rviz/panel.h>
#endif
#include <QLabel>
#include <QPushButton>
namespace cam_lidar_calibration
......@@ -43,6 +44,7 @@ protected:
ros::NodeHandle nh_;
ros::ServiceClient optimise_client_;
QLabel* output_label_;
QPushButton* capture_button_;
QPushButton* discard_button_;
QPushButton* optimise_button_;
......
......@@ -16,10 +16,12 @@
#include <pcl/segmentation/impl/sac_segmentation.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Geometry>
#include <cv_bridge/cv_bridge.h>
#include <eigen_conversions/eigen_msg.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/image_encodings.h>
#include <visualization_msgs/MarkerArray.h>
......@@ -86,7 +88,14 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
if (!optimiser_->samples_.empty())
{
ROS_INFO("Calling optimiser");
optimiser_->optimise();
auto t = optimiser_->optimise();
res.transform.translation.x = t.x / 1000.;
res.transform.translation.y = t.y / 1000.;
res.transform.translation.z = t.z / 1000.;
Eigen::Matrix3d mat;
cv::cv2eigen(t.rot.toMat(), mat);
Eigen::Quaterniond quat(mat);
tf::quaternionEigenToMsg(quat, res.transform.rotation);
}
break;
}
......
......@@ -275,7 +275,7 @@ void Optimiser::SO_report_generation(int generation_number,
best_rotation_.yaw = best_genes.yaw;
}
bool Optimiser::optimise()
RotationTranslation Optimiser::optimise()
{
auto n = samples_.size();
ROS_INFO_STREAM("Optimising " << n << " collected samples");
......@@ -473,7 +473,7 @@ bool Optimiser::optimise()
<< rot_trans.x / 1000.0 << " " << rot_trans.y / 1000.0 << " " << rot_trans.z / 1000.0);
ROS_INFO_STREAM("The problem was optimised in " << timer.toc() << " seconds");
return true;
return rot_trans;
}
// Function converts rotation matrix to corresponding euler angles
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment