bool CameraLidarTransformer::transformServiceCallback(
navigator_msgs::CameraToLidarTransform::Request &req,
navigator_msgs::CameraToLidarTransform::Response &res) {
visualization_msgs::MarkerArray markers;
sensor_msgs::PointCloud2ConstPtr scloud =
lidarCache.getElemAfterTime(req.header.stamp);
if (!scloud) {
res.success = false;
res.error = navigator_msgs::CameraToLidarTransform::Response::CLOUD_NOT_FOUND;
return true;
}
geometry_msgs::TransformStamped transform =
tfBuffer.lookupTransform(req.header.frame_id, "velodyne", req.header.stamp);
sensor_msgs::PointCloud2 cloud_transformed;
tf2::doTransform(*scloud, cloud_transformed, transform);
#ifdef DO_ROS_DEBUG
cv::Mat debug_image(camera_info.height, camera_info.width, CV_8UC3,
cv::Scalar(0));
#endif
Eigen::Matrix3d matrixFindPlaneA;
matrixFindPlaneA << 0, 0, 0, 0, 0, 0, 0, 0, 0;
Eigen::Vector3d matrixFindPlaneB(0, 0, 0);
cv::circle(debug_image, cv::Point(req.point.x, req.point.y), 8,
cv::Scalar(255, 0, 0), -1);
//Tracks the closest lidar point to the requested camera point
double minDistance = std::numeric_limits<double>::max();
for (auto ii = 0, jj = 0; ii < cloud_transformed.width;
++ii, jj += cloud_transformed.point_step) {
floatConverter x, y, z, i;
for (int kk = 0; kk < 4; ++kk) {
x.data[kk] = cloud_transformed.data[jj + kk];
y.data[kk] = cloud_transformed.data[jj + 4 + kk];
z.data[kk] = cloud_transformed.data[jj + 8 + kk];
i.data[kk] = cloud_transformed.data[jj + 16 + kk];
}
cam_model.fromCameraInfo(camera_info);
if (int(z.f) < 0 || int(z.f) > 30) continue;
cv::Point2d point = cam_model.project3dToPixel(cv::Point3d(x.f, y.f, z.f));
if (int(point.x) > 0 && int(point.x) < camera_info.width &&
int(point.y) > 0 && int(point.y) < camera_info.height) {
#ifdef DO_ROS_DEBUG
visualization_msgs::Marker marker_point;
marker_point.header = req.header;
marker_point.header.seq = 0;
marker_point.header.frame_id = req.header.frame_id;
marker_point.id = (int)ii;
marker_point.type = visualization_msgs::Marker::CUBE;
marker_point.pose.position.x = x.f;
marker_point.pose.position.y = y.f;
marker_point.pose.position.z = z.f;
marker_point.scale.x = 1;
marker_point.scale.y = 0.1;
marker_point.scale.z = 0.1;
marker_point.color.a = 1.0;
marker_point.color.r = 0.0;
marker_point.color.g = 1.0;
marker_point.color.b = 0.0;
//marker_point.lifetime = ros::Duration(0.5);
markers.markers.push_back(marker_point);
// Draw
if (int(point.x - 20) > 0 && int(point.x + 20) < camera_info.width &&
int(point.y - 20) > 0 && int(point.y + 20) < camera_info.height) {
cv::circle(debug_image, point, 3,
cv::Scalar(0, 0, 255 * std::min(1.0, z.f / 20.0)), -1);
}
#endif
double distance = sqrt(pow(point.x - req.point.x ,2) + pow(point.y - req.point.y,2) ); //Distance (2D) from request point to projected lidar point
if (distance < req.tolerance) {
#ifdef DO_ROS_DEBUG
if (int(point.x - 20) > 0 && int(point.x + 20) < camera_info.width &&
int(point.y - 20) > 0 && int(point.y + 20) < camera_info.height) {
cv::circle(debug_image, point, 3, cv::Scalar(0, 255, 0), -1);
}
#endif
matrixFindPlaneA(0, 0) += x.f * x.f;
matrixFindPlaneA(1, 0) += y.f * x.f;
matrixFindPlaneA(2, 0) += x.f;
matrixFindPlaneA(0, 1) += x.f * y.f;
matrixFindPlaneA(1, 1) += y.f * y.f;
matrixFindPlaneA(2, 1) += y.f;
matrixFindPlaneA(0, 2) += x.f;
matrixFindPlaneA(1, 2) += y.f;
matrixFindPlaneA(2, 2) += 1;
matrixFindPlaneB(0, 0) -= x.f * z.f;
matrixFindPlaneB(1, 0) -= y.f * z.f;
matrixFindPlaneB(2, 0) -= z.f;
geometry_msgs::Point geo_point;
geo_point.x = x.f;
geo_point.y = y.f;
geo_point.z = z.f;
if (distance < minDistance)
{
res.closest = geo_point;
minDistance = distance;
}
res.transformed.push_back(geo_point);
}
}
}
if (res.transformed.size() < 1) {
res.success = false;
res.error = navigator_msgs::CameraToLidarTransform::Response::NO_POINTS_FOUND;
return true;
}
//Filter out lidar points behind the plane (I don't think this will work, no good way to do this)
// ~double min_z = (*std::min_element(res.transformed.begin(),res.transformed.end(), [=] (const geometry_msgs::Point& a,const geometry_msgs::Point& b) {
// ~return a.z < b.z && a.z > MIN_Z;
// ~})).z;
// ~res.transformed.erase(std::remove_if(res.transformed.begin(),res.transformed.end(),[min_z] (const geometry_msgs::Point& p) {
// ~return (p.z - min_z) < MAX_Z_ERROR;
// ~}),res.transformed.end());
res.distance = res.closest.z;
Eigen::Vector3d normalvec = matrixFindPlaneA.colPivHouseholderQr()
.solve(matrixFindPlaneB)
.normalized();
geometry_msgs::Vector3 normalvec_ros;
normalvec_ros.x = normalvec(0, 0);
normalvec_ros.y = normalvec(1, 0);
normalvec_ros.z = normalvec(2, 0);
res.normal = normalvec_ros;
std::cout << "Plane solution: " << normalvec << std::endl;
#ifdef DO_ROS_DEBUG
//Create marker for normal
visualization_msgs::Marker marker_normal;
marker_normal.header = req.header;
marker_normal.header.seq = 0;
marker_normal.header.frame_id = req.header.frame_id;
marker_normal.id = 3000;
marker_normal.type = visualization_msgs::Marker::ARROW;
geometry_msgs::Point sdp_normalvec_ros;
sdp_normalvec_ros.x = normalvec_ros.x + res.transformed[0].x;
sdp_normalvec_ros.y = normalvec_ros.y + res.transformed[0].y;
sdp_normalvec_ros.z = normalvec_ros.z + res.transformed[0].z;
marker_normal.points.push_back(res.closest);
marker_normal.points.push_back(sdp_normalvec_ros);
marker_normal.scale.x = 0.1;
marker_normal.scale.y = 0.5;
marker_normal.scale.z = 0.5;
marker_normal.color.a = 1.0;
marker_normal.color.r = 1.0;
marker_normal.color.g = 0.0;
marker_normal.color.b = 0.0;
markers.markers.push_back(marker_normal);
//Publish 3D debug market
pubMarkers.publish(markers);
//Publish debug image
cv_bridge::CvImage ros_debug_image;
ros_debug_image.encoding = "bgr8";
ros_debug_image.image = debug_image.clone();
points_debug_publisher.publish(ros_debug_image.toImageMsg());
#endif
res.success = true;
return true;
}
void mesh_core::Plane::leastSquaresFast(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
Eigen::Matrix3d m;
Eigen::Vector3d b;
Eigen::Vector3d c;
m.setZero();
b.setZero();
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
{
m(0,0) += p->x() * p->x();
m(1,0) += p->x() * p->y();
m(2,0) += p->x();
m(1,1) += p->y() * p->y();
m(2,1) += p->y();
b(0) += p->x() * p->z();
b(1) += p->y() * p->z();
b(2) += p->z();
c += *p;
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
m(2,2) = double(points.size());
c *= 1.0/double(points.size());
normal_ = m.colPivHouseholderQr().solve(b);
if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
normal_.normalize();
d_ = -c.dot(normal_);
if (average)
*average = c;
}
std::vector<double>
jerkMinimizingTrajectory(const motion& state0, const motion& state1, double dt) {
double dt2 = dt*dt;
double dt3 = dt2*dt;
double dt4 = dt3*dt;
double dt5 = dt4*dt;
Eigen::Matrix3d a;
a << dt3, dt4, dt5,
3*dt2, 4*dt3, 5*dt4,
6*dt, 12*dt2, 20*dt3;
Eigen::Vector3d b;
b << state1[0] - (state0[0] + dt*state0[1] + 0.5*dt2*state0[2]),
state1[1] - (state0[1] + dt*state0[2]),
state1[2] - state0[2];
Eigen::VectorXd p = a.colPivHouseholderQr().solve(b);
return {state0[0], state0[1], state0[2]/2.0, p[0], p[1], p[2]};
}
Types::Transform PointOnPlaneCalibration::estimateTransform(const std::vector<PointPlanePair> & pair_vector)
{
const int size = pair_vector.size();
// Point-Plane Constraints
// Initial system (Eq. 10)
Eigen::MatrixXd system(size, 13);
for (int i = 0; i < size; ++i)
{
const double d = pair_vector[i].plane_.offset();
const Eigen::Vector3d n = pair_vector[i].plane_.normal();
const Types::Point3 & x = pair_vector[i].point_;
system.row(i) << d + n[0] * x[0] + n[1] * x[1] + n[2] * x[2], // q_0^2
2 * n[2] * x[1] - 2 * n[1] * x[2], // q_0 * q_1
2 * n[0] * x[2] - 2 * n[2] * x[0], // q_0 * q_2
2 * n[1] * x[0] - 2 * n[0] * x[1], // q_0 * q_3
d + n[0] * x[0] - n[1] * x[1] - n[2] * x[2], // q_1^2
2 * n[0] * x[1] + 2 * n[1] * x[0], // q_1 * q_2
2 * n[0] * x[2] + 2 * n[2] * x[0], // q_1 * q_3
d - n[0] * x[0] + n[1] * x[1] - n[2] * x[2], // q_2^2
2 * n[1] * x[2] + 2 * n[2] * x[1], // q_2 * q_3
d - n[0] * x[0] - n[1] * x[1] + n[2] * x[2], // q_3^2
n[0], n[1], n[2]; // q'*q*t
}
// Gaussian reduction
for (int k = 0; k < 3; ++k)
for (int i = k + 1; i < size; ++i)
system.row(i) = system.row(i) - system.row(k) * system.row(i)[10 + k] / system.row(k)[10 + k];
// Quaternion q
Eigen::Vector4d q;
// Transform to inhomogeneous (Eq. 13)
bool P_is_ok(false);
while (not P_is_ok)
{
Eigen::Matrix4d P(Eigen::Matrix4d::Random());
while (P.determinant() < 1e-5)
P = Eigen::Matrix4d::Random();
Eigen::MatrixXd reduced_system(size - 3, 10);
for (int i = 3; i < size; ++i)
{
const Eigen::VectorXd & row = system.row(i);
Eigen::Matrix4d Mi_tilde;
Mi_tilde << row[0] , row[1] / 2, row[2] / 2, row[3] / 2,
row[1] / 2, row[4] , row[5] / 2, row[6] / 2,
row[2] / 2, row[5] / 2, row[7] , row[8] / 2,
row[3] / 2, row[6] / 2, row[8] / 2, row[9] ;
Eigen::Matrix4d Mi_bar(P.transpose() * Mi_tilde * P);
reduced_system.row(i - 3) << Mi_bar(0, 0),
Mi_bar(0, 1) + Mi_bar(1, 0),
Mi_bar(0, 2) + Mi_bar(2, 0),
Mi_bar(0, 3) + Mi_bar(3, 0),
Mi_bar(1, 1),
Mi_bar(1, 2) + Mi_bar(2, 1),
Mi_bar(1, 3) + Mi_bar(3, 1),
Mi_bar(2, 2),
Mi_bar(2, 3) + Mi_bar(3, 2),
Mi_bar(3, 3);
}
// Solve A m* = b
Eigen::MatrixXd A = reduced_system.rightCols<9>();
Eigen::VectorXd b = - reduced_system.leftCols<1>();
Eigen::VectorXd m_star = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
Eigen::Vector4d q_bar(1, m_star[0], m_star[1], m_star[2]);
Eigen::VectorXd err(6);
err << q_bar[1] * q_bar[1],
q_bar[1] * q_bar[2],
q_bar[1] * q_bar[3],
q_bar[2] * q_bar[2],
q_bar[2] * q_bar[3],
q_bar[3] * q_bar[3];
err -= m_star.tail<6>();
//if (err.norm() < 0.1) // P is not ok?
P_is_ok = true;
//std::cout << m_star.transpose() << std::endl;
//std::cout << q_bar[1] * q_bar[1] << " " << q_bar[1] * q_bar[2] << " " << q_bar[1] * q_bar[3] << " "
//<< q_bar[2] * q_bar[2] << " " << q_bar[2] * q_bar[3] << " " << q_bar[3] * q_bar[3] << std::endl;
q = P * q_bar;
}
// We want q.w > 0 (Why?)
//if (q[0] < 0)
// q = -q;
Eigen::Vector4d tmp_q(q.normalized());
Eigen::Quaterniond rotation(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3]);
Eigen::VectorXd m(10);
m << q[0] * q[0],
q[0] * q[1],
q[0] * q[2],
q[0] * q[3],
q[1] * q[1],
q[1] * q[2],
q[1] * q[3],
q[2] * q[2],
q[2] * q[3],
q[3] * q[3];
// Solve A (q^T q t) = b
Eigen::Matrix3d A = system.topRightCorner<3, 3>();
Eigen::Vector3d b = - system.topLeftCorner<3, 10>() * m;
Eigen::Translation3d translation(A.colPivHouseholderQr().solve(b) / q.squaredNorm());
// Eigen::Quaterniond tmp(pose.linear());
// Eigen::Translation3d tmp2(pose.translation());
// std::cout << "Prev: " << tmp.normalized().coeffs().transpose() << " " << tmp2.vector().transpose() << std::endl;
//std::cout << "PoP : " << rotation.coeffs().transpose() << " " << translation.vector().transpose() << std::endl;
return translation * rotation;
}