Eigen::Vector3d PointcloudProc::projectPoint(Eigen::Vector4d& point, CamParams cam) const
{
Eigen::Vector3d keypoint;
keypoint(0) = (cam.fx*point.x()) / point.z() + cam.cx;
keypoint(1) = (cam.fy*point.y()) / point.z() + cam.cy;
keypoint(2) = (cam.fx*(point.x()-cam.tx)/point.z() + cam.cx);
return keypoint;
}
Visualization::Visualization(bot_lcmgl_t* lcmgl, const StereoCalibration* calib)
: _lcmgl(lcmgl),
_calibration(calib)
{
// take the Z corresponding to disparity 5 px as 'max Z'
Eigen::Matrix4d uvdtoxyz = calib->getUvdToXyz();
Eigen::Vector4d xyzw = uvdtoxyz * Eigen::Vector4d(1, 1, 5, 1);
xyzw /= xyzw.w();
_max_z = xyzw.z();
// take the Z corresponding to 3/4 disparity img width px as 'min Z'
xyzw = uvdtoxyz * Eigen::Vector4d(1, 1, (3*calib->getWidth())/4, 1);
xyzw /= xyzw.w();
_min_z = xyzw.z();
}
void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
{
if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) )
{
try
{
// update the pose
listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map);
listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map);
tf::Vector3 position_( kinect2map.getOrigin() );
position.x() = position_.x();
position.y() = position_.y();
position.z() = position_.z();
tf::Quaternion orientation_( kinect2map.getRotation() );
orientation.x() = orientation_.x();
orientation.y() = orientation_.y();
orientation.z() = orientation_.z();
orientation.w() = orientation_.w();
ROS_INFO_STREAM("position = " << position.transpose() );
ROS_INFO_STREAM("orientation = " << orientation.transpose() );
// update the cloud
pcl::copyPointCloud(*cloud, *xyz_cld_ptr); // Do I need to copy it?
//xyz_cld_ptr = cloud;
cloud_updated = true;
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
}
}
void publishPointMarker(ros::Publisher &vis_pub, const Eigen::Vector4d &p, int ID)
{
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time::now();
marker.ns = "point";
marker.id = ID;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDE
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = p.x();
marker.pose.position.y = p.y();
marker.pose.position.z = p.z();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.05;
marker.scale.y = 0.05;
marker.scale.z = 0.05;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.lifetime = ros::Duration();
vis_pub.publish(marker);
}
int main(int /*argc*/, char** /*argv*/) {
Line3D l0;
std::cout << "l0: " << std::endl;
printPlueckerLine(std::cout, l0);
std::cout << std::endl;
Vector6d v;
v << 1.0, 1.0, -0.3, 0.5, 0.2, 0.3;
Line3D l1(v);
l1.normalize();
std::cout << "v: ";
printVector(std::cout, v);
std::cout << std::endl;
std::cout << "l1: " << std::endl;
printPlueckerLine(std::cout, l1);
std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
std::cout << std::endl;
Line3D l2(l1);
std::cout << "l2: " << std::endl;
printPlueckerLine(std::cout, l2);
std::cout << std::endl;
Eigen::Vector4d mv = l2.ominus(l1);
Eigen::Quaterniond q(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
double yaw = euler_angles[0];
double pitch = euler_angles[1];
double roll = euler_angles[2];
std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
std::cout << std::endl;
v << 0.0, 0.0, 1.0, 0.5, 0.5, 0.0;
l1 = Line3D(v);
l1.normalize();
std::cout << "l1: " << std::endl;
printPlueckerLine(std::cout, l1);
std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
std::cout << std::endl;
v << 0.0, 0.0, 1.0, 0.5, -0.5, 0.0;
l2 = Line3D(v);
l2.normalize();
std::cout << "l2: " << std::endl;
printPlueckerLine(std::cout, l2);
std::cout << "azimuth: " << g2o::internal::getAzimuth(l2.d()) << std::endl;
std::cout << "elevation: " << g2o::internal::getElevation(l2.d()) << std::endl;
std::cout << std::endl;
mv = l2.ominus(l1);
q = Eigen::Quaterniond(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler_angles[0];
pitch = euler_angles[1];
roll = euler_angles[2];
std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
std::cout << std::endl;
Line3D l3 = Line3D(l1);
std::cout << "l3: " << std::endl;
printPlueckerLine(std::cout, l3);
l3.oplus(mv);
std::cout << "l3 oplus mv: " << std::endl;
printPlueckerLine(std::cout, l3);
std::cout << std::endl;
std::vector<Line3D> l;
v << 0.0, 0.0, 1.0, 1.0, 0.0, 0.0;
Line3D ll = Line3D(v);
ll.normalize();
l.push_back(ll);
v << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0;
ll = Line3D(v);
ll.normalize();
l.push_back(ll);
v << 0.0, 0.0, 1.0, 0.0, 0.0, 1.0;
ll = Line3D(v);
ll.normalize();
l.push_back(ll);
for(size_t i = 0; i < l.size(); ++i) {
Line3D& line = l[i];
std::cout << "line: "
<< line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " "
<< line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl;
}
std::cout << std::endl;
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translation().x() = 0.9;
std::cout << "R: " << std::endl << T.linear() << std::endl;
std::cout << "t: " << std::endl << T.translation() << std::endl;
std::cout << std::endl;
for(size_t i = 0; i < l.size(); ++i) {
Line3D& line = l[i];
line = T * line;
std::cout << "line: "
<< line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " "
<< line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl;
}
std::cout << std::endl;
return 0;
}