TEST(TFEigenConversions, tf_eigen_transform)
{
tf::Transform t;
tf::Quaternion tq;
tq[0] = gen_rand(-1.0,1.0);
tq[1] = gen_rand(-1.0,1.0);
tq[2] = gen_rand(-1.0,1.0);
tq[3] = gen_rand(-1.0,1.0);
tq.normalize();
t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
t.setRotation(tq);
Eigen::Affine3d k;
transformTFToEigen(t,k);
for(int i=0; i < 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
}
}
for (int col = 0 ; col < 3; col ++)
ASSERT_NEAR(k.matrix()(3, col), 0, 1e-6);
ASSERT_NEAR(k.matrix()(3,3), 1, 1e-6);
}
void drawRedDotsPlane()
{
int planeTilt = (int)factors.at("Tilt");
int planeSlant = (int)factors.at("Slant");
double planeDef = factors.at("Def");
double planeOnset=1;
bool backprojectionActive=true;
double theta=0.0;
Eigen::Affine3d modelTransformation;
glPushMatrix();
//glLoadIdentity();
//glTranslated(0,0,focalDistance);
glMultMatrixd(stimTransformation.data());
switch ( planeTilt )
{
case 0:
{
theta = -acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1-planeOnset*periodicValue/oscillationAmplitude)));
glRotated(toDegrees(theta),0,1,0);
if (backprojectionActive)
glScaled(1/sin(toRadians( -90-planeSlant)),1,1); //backprojection phase
}
break;
case 90:
{
theta = -acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1-planeOnset*periodicValue/oscillationAmplitude)));
glRotated( toDegrees(theta),1,0,0);
if (backprojectionActive)
glScaled(1,1/sin(toRadians( -90-planeSlant )),1); //backprojection phase
}
break;
case 180:
{
theta = acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1+planeOnset*periodicValue/oscillationAmplitude)));
glRotated(toDegrees(theta),0,1,0);
if (backprojectionActive)
glScaled(1/sin(toRadians( -90-planeSlant )),1,1); //backprojection phase
}
break;
case 270:
{
theta = acos(exp(-0.346574+0.5*planeDef-planeDef/2*(1+planeOnset*periodicValue/oscillationAmplitude)));
glRotated( toDegrees(theta),1,0,0);
if (backprojectionActive)
glScaled(1,1/sin(toRadians( -90-planeSlant )),1); //backprojection phase
}
break;
default:
cerr << "Error, select tilt to be {0,90,180,270} only" << endl;
}
stimDrawer.draw();
glGetDoublev(GL_MODELVIEW_MATRIX,modelTransformation.matrix().data());
modelTransformation.matrix() = modelTransformation.matrix().eval();
glPopMatrix();
}
Eigen::Affine3d transformTFToEigen(const tf::Transform &t) {
Eigen::Affine3d e;
for (int i = 0; i < 3; i++) {
e.matrix()(i, 3) = t.getOrigin()[i];
for (int j = 0; j < 3; j++) {
e.matrix()(i, j) = t.getBasis()[i][j];
}
}
// Fill in identity in last row
for (int col = 0; col < 3; col++)
e.matrix()(3, col) = 0;
e.matrix()(3, 3) = 1;
return e;
}
Eigen::Affine3d transformTFToEigen(const tf::Transform &t) {
Eigen::Affine3d e;
// treat the Eigen::Affine as a 4x4 matrix:
for (int i = 0; i < 3; i++) {
e.matrix()(i, 3) = t.getOrigin()[i]; //copy the origin from tf to Eigen
for (int j = 0; j < 3; j++) {
e.matrix()(i, j) = t.getBasis()[i][j]; //and copy 3x3 rotation matrix
}
}
// Fill in identity in last row
for (int col = 0; col < 3; col++)
e.matrix()(3, col) = 0;
e.matrix()(3, 3) = 1;
return e;
}
void TSDFVolumeOctree::getFrustumCulledVoxels (const Eigen::Affine3d& trans,
std::vector<OctreeNode::Ptr> &voxels) const {
std::vector<OctreeNode::Ptr> voxels_all;
octree_->getLeaves (voxels_all, max_cell_size_x_, max_cell_size_y_, max_cell_size_z_);
pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_cloud(new pcl::PointCloud<pcl::PointXYZ> (voxels_all.size(), 1));
std::vector<int> indices;
for (size_t i = 0; i < voxel_cloud->size(); ++i) {
pcl::PointXYZ &pt = voxel_cloud->at (i);
voxels_all[i]->getCenter (pt.x, pt.y, pt.z);
}
pcl::FrustumCulling<pcl::PointXYZ> fc (false);
Eigen::Matrix4f cam2robot;
cam2robot << 0, 0, 1, 0,
0, -1, 0, 0,
1, 0, 0, 0,
0, 0, 0, 1;
Eigen::Matrix4f trans_robot = trans.matrix().cast<float> () * cam2robot;
fc.setCameraPose (trans_robot);
fc.setHorizontalFOV (70);
fc.setVerticalFOV (70);
fc.setNearPlaneDistance (min_sensor_dist_);
fc.setFarPlaneDistance (max_sensor_dist_);
fc.setInputCloud (voxel_cloud);
fc.filter (indices);
voxels.resize (indices.size());
for (size_t i = 0; i < indices.size(); ++i) {
voxels[i] = voxels_all[indices[i]];
}
}
bool srvSetHuboObjectPose(HuboApplication::SetHuboObjectPose::Request &req,
HuboApplication::SetHuboObjectPose::Response &res)
{
bool response = true;
Eigen::Affine3d tempPose;
Eigen::Isometry3d armPose;
tf::poseMsgToEigen(req.Target, tempPose);
if (tempPose(0,3) != tempPose(0,3)) // null check
{
res.Success = false;
return false;
}
armPose = tempPose.matrix();
//std::cerr << tempPose.matrix() << std::endl;
m_Manip.setControlMode(OBJECT_POSE);
m_Manip.setAngleMode(QUATERNION);
m_Manip.setPose(armPose, req.ObjectIndex);
m_Manip.sendCommand();
//std::cerr << armPose.matrix() << std::endl;
res.Success = response;
return response;
}
bool srvSetHuboArmPose(HuboApplication::SetHuboArmPose::Request &req,
HuboApplication::SetHuboArmPose::Response &res)
{
bool response = true;
Eigen::Affine3d tempPose;
Eigen::Isometry3d armPose;
tf::poseMsgToEigen(req.Target, tempPose);
if (tempPose(0,3) != tempPose(0,3)) // null check
{
res.Success = false;
return false;
}
//std::cerr << tempPose.matrix() << std::endl;
armPose = tempPose.matrix();
//armPose.translation() = tempPose.translation();
//armPose.linear() = tempPose.rotation();
//std::cerr << armPose.matrix() << std::endl;
m_Manip.setControlMode(END_EFFECTOR);
m_Manip.setAngleMode(QUATERNION);
m_Manip.setPose(armPose, req.ArmIndex);
m_Manip.sendCommand();
//std::cerr << armPose.matrix() << std::endl;
res.Success = response;
return response;
}
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
{
t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1),e.matrix()(0,2),
e.matrix()(1,0), e.matrix()(1,1),e.matrix()(1,2),
e.matrix()(2,0), e.matrix()(2,1),e.matrix()(2,2)));
}
void Data4Viewer::drawGlobalView()
{
_pKinect->_pRGBCamera->setGLProjectionMatrix(0.1f, 100.f);
glMatrixMode(GL_MODELVIEW);
Eigen::Affine3d tmp; tmp.setIdentity();
Eigen::Matrix4d mMat;
mMat.row(0) = tmp.matrix().row(0);
mMat.row(1) = -tmp.matrix().row(1);
mMat.row(2) = -tmp.matrix().row(2);
mMat.row(3) = tmp.matrix().row(3);
glLoadMatrixd(mMat.data());
GpuMat rgb(_pKinect->_cvmRGB);
_pKinect->_pRGBCamera->renderCameraInLocal(rgb, _pGL.get(), false, NULL, 0.2f, true); //render in model coordinate
//cout<<("drawRGBView");
return;
}
void visualOdometry::mainloop(){
FRAME lastFrame;
std::string file_name_ground =pd_.getData( "file_name_ground");
readFromFile( file_name_ground,ground_truth_list_);
lastFrame = readFrameFromGroundTruthList(start_index_ ,ground_truth_list_);
// 我们总是在比较currFrame和lastFrame
string detector = pd_.getData( "detector" );
string descriptor = pd_.getData( "descriptor" );
CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
computeKeyPointsAndDesp( lastFrame, detector, descriptor );
// 是否显示点云
bool visualize = pd_.getData("visualize_pointcloud")==string("true");
int min_inliers = atoi( pd_.getData("min_inliers").c_str() );
double max_norm = atof( pd_.getData("max_norm").c_str() );
for (int currIndex=start_index_+1; currIndex<end_index_; currIndex++ )
{
current_index_ = currIndex;
cout<<"Reading files "<<currIndex<<endl;
FRAME currFrame = readFrameFromGroundTruthList(currIndex, ground_truth_list_); // 读取currFrame
computeKeyPointsAndDesp( currFrame, detector, descriptor );
// 比较currFrame 和 lastFrame
RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
if ( result.inliers < min_inliers ){ //inliers不够，放弃该帧
std::cout<<"not enouth inliers: "<<result.inliers;
continue;
}
//ROS_INFO_STREAM("trans: "<< result.tvec <<" rot: "<< result.rvec);
cv::Mat rotation_matrix;
cv::Rodrigues(result.rvec, rotation_matrix);
Eigen::Matrix3d rotation_eigen_temp;
cv::cv2eigen(rotation_matrix,rotation_eigen_temp);
Eigen::AngleAxisd rotation_eigen(rotation_eigen_temp);
Eigen::Affine3d incrementalAffine = Eigen::Affine3d::Identity();
incrementalAffine = rotation_eigen;
incrementalAffine(0,3) = result.tvec.ptr<double>(0)[0];
incrementalAffine(1,3) = result.tvec.ptr<double>(1)[0];
incrementalAffine(2,3) = result.tvec.ptr<double>(2)[0];
//pose_camera_ = incrementalAffine * pose_camera_;
publishTrajectory();
publishGroundTruth();
ROS_INFO_STREAM("RT: "<< incrementalAffine.matrix());
lastFrame = currFrame;
}
}
Plane Plane::transform(const Eigen::Affine3d& transform)
{
Eigen::Vector4d n;
n[0] = normal_[0];
n[1] = normal_[1];
n[2] = normal_[2];
n[3] = d_;
Eigen::Matrix4d m = transform.matrix();
Eigen::Vector4d n_d = m.transpose() * n;
Eigen::Vector4d n_dd = n_d.normalized();
return Plane(Eigen::Vector3d(n_dd[0], n_dd[1], n_dd[2]), n_dd[3]);
}
TEST(TFEigenConversions, eigen_tf_transform)
{
tf::Transform t;
Eigen::Affine3d k;
Eigen::Quaterniond kq;
kq.coeffs()(0) = gen_rand(-1.0,1.0);
kq.coeffs()(1) = gen_rand(-1.0,1.0);
kq.coeffs()(2) = gen_rand(-1.0,1.0);
kq.coeffs()(3) = gen_rand(-1.0,1.0);
kq.normalize();
k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
k.rotate(kq);
transformEigenToTF(k,t);
for(int i=0; i < 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
}
}
}
void idle()
{
Timer frameTimer; frameTimer.start();
// Timing things
timeFrame+=1;
double oscillationPeriod = factors.at("StimulusDuration")*TIMER_MS;
switch (stimMotion)
{
case SAWTOOTH_MOTION:
periodicValue = oscillationAmplitude*mathcommon::sawtooth(timeFrame,oscillationPeriod);
break;
case TRIANGLE_MOTION:
periodicValue = oscillationAmplitude*mathcommon::trianglewave(timeFrame,oscillationPeriod);
break;
case SINUSOIDAL_MOTION:
periodicValue = oscillationAmplitude*sin(3.14*timeFrame/(oscillationPeriod));
break;
default:
SAWTOOTH_MOTION;
}
timingFile << totalTimer.getElapsedTimeInMilliSec() << " " << periodicValue << endl;
// Simulate head translation
// Coordinates picker
markers[1] = Vector3d(0,0,0);
markers[2] = Vector3d(0,10,0);
markers[3] = Vector3d(0,0,10);
headEyeCoords.update(markers[1],markers[2],markers[3]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
Vector3d fixationPoint = (headEyeCoords.getRigidStart().getFullTransformation() * ( Vector3d(0,0,focalDistance) ) );
// Projection of view normal on the focal plane
Eigen::ParametrizedLine<double,3> pline = Eigen::ParametrizedLine<double,3>::Through(eyeRight,fixationPoint);
projPoint = pline.intersection(focalPlane)*((fixationPoint - eyeRight).normalized()) + eyeRight;
stimTransformation.matrix().setIdentity();
stimTransformation.translation() <<0,0,focalDistance;
Timer sleepTimer;
sleepTimer.sleep((TIMER_MS - frameTimer.getElapsedTimeInMilliSec())/2);
}
Eigen::Matrix4d getTranformationMatrix(const string &frameName, const string &coordSys ){
tf::TransformListener listener;
ros::Time ts(0) ;
tf::StampedTransform transform;
Eigen::Affine3d pose;
try {
listener.waitForTransform( coordSys, frameName, ts, ros::Duration(1) );
listener.lookupTransform( coordSys, frameName, ts, transform);
tf::TransformTFToEigen(transform, pose);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
return pose.matrix();
}
TEST(TfEigen, ConvertTransform)
{
Eigen::Matrix4d tm;
double alpha = M_PI/4.0;
double theta = M_PI/6.0;
double gamma = M_PI/12.0;
tm << cos(theta)*cos(gamma),-cos(theta)*sin(gamma),sin(theta), 1, //
cos(alpha)*sin(gamma)+sin(alpha)*sin(theta)*cos(gamma),cos(alpha)*cos(gamma)-sin(alpha)*sin(theta)*sin(gamma),-sin(alpha)*cos(theta), 2, //
sin(alpha)*sin(gamma)-cos(alpha)*sin(theta)*cos(gamma),cos(alpha)*sin(theta)*sin(gamma)+sin(alpha)*cos(gamma),cos(alpha)*cos(theta), 3, //
0, 0, 0, 1;
Eigen::Affine3d T(tm);
geometry_msgs::TransformStamped msg = tf2::eigenToTransform(T);
Eigen::Affine3d Tback = tf2::transformToEigen(msg);
EXPECT_TRUE(T.isApprox(Tback));
EXPECT_TRUE(tm.isApprox(Tback.matrix()));
}
static bool colorize(const drc::map_image_t& iDepthMap,
const Eigen::Affine3d& iLocalToCamera,
const bot_core::image_t& iImage,
const BotCamTrans* iCamTrans,
bot_core::image_t& oImage) {
oImage.utime = iDepthMap.utime;
oImage.width = iDepthMap.width;
oImage.height = iDepthMap.height;
oImage.row_stride = 3*iDepthMap.width;
oImage.pixelformat = PIXEL_FORMAT_RGB;
oImage.size = oImage.row_stride * oImage.height;
oImage.nmetadata = 0;
oImage.data.resize(oImage.size);
Eigen::Matrix4d xform;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
xform(i,j) = iDepthMap.transform[i][j];
}
}
xform = iLocalToCamera.matrix()*xform.inverse();
for (int i = 0; i < iDepthMap.height; ++i) {
for (int j = 0; j < iDepthMap.width; ++j) {
double z;
if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
if (z < -1e10) {
continue;
}
}
else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
if (val == 0) {
continue;
}
z = val;
}
else {
continue;
}
Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
double p[3] = {pt(0)/pt(3),pt(1)/pt(3),pt(2)/pt(3)};
double pix[3];
bot_camtrans_project_point(iCamTrans, p, pix);
if (pix[2] < 0) {
continue;
}
uint8_t r,g,b;
if (!interpolate(pix[0], pix[1], iImage, r, g, b)) {
continue;
}
int outImageIndex = i*oImage.row_stride + 3*j;
oImage.data[outImageIndex+0] = r;
oImage.data[outImageIndex+1] = g;
oImage.data[outImageIndex+2] = b;
}
}
return true;
}
void idle()
{
if (totalTimer.getElapsedTimeInSec() > initialAdaptationTimeInSeconds
&& trialMode == INITIALADAPTATION && experimentStarted )
{
trialMode = STIMULUSMODE;
totalTimer.start();
}
if (totalTimer.getElapsedTimeInSec() > fixationDurationInSeconds && trialMode == FIXATIONMODE && experimentStarted )
{
timeFrame=0.0;
trialMode=STIMULUSMODE;
totalTimer.start();
}
if (totalTimer.getElapsedTimeInMilliSec() > stimulusDurationInMilliSeconds && trialMode == STIMULUSMODE && experimentStarted )
{
timeFrame=0.0;
trialMode=PROBEMODE;
totalTimer.start();
}
if (!experimentStarted)
return;
Timer frameTimer;
frameTimer.start();
// Timing things
double oscillationPeriod = stimulusDurationInMilliSeconds;
switch (stimMotion)
{
case SAWTOOTH_MOTION:
periodicValue = oscillationAmplitude*mathcommon::sawtooth(timeFrame,oscillationPeriod);
break;
case TRIANGLE_MOTION:
periodicValue = oscillationAmplitude*mathcommon::trianglewave(timeFrame,oscillationPeriod);
break;
case SINUSOIDAL_MOTION:
periodicValue = oscillationAmplitude*sin(3.14*timeFrame/(oscillationPeriod));
break;
default:
SAWTOOTH_MOTION;
}
timingFile << totalTimer.getElapsedTimeInMilliSec() << " " << periodicValue << endl;
// Simulate head translation
// Coordinates picker
markers[1] = Vector3d(0,0,0);
markers[2] = Vector3d(0,10,0);
markers[3] = Vector3d(0,0,10);
headEyeCoords.update(markers[1],markers[2],markers[3]);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
stimTransformation.matrix().setIdentity();
stimTransformation.translation() <<0,0,focalDistance;
Timer sleepTimer;
sleepTimer.sleep(abs(TIMER_MS - frameTimer.getElapsedTimeInMilliSec()));
timeFrame+=1;
}
bool
pcl::EnsensoGrabber::computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
std::string &json,
const std::string setup,
const std::string target,
const Eigen::Affine3d &guess_tf,
const bool pretty_format) const
{
try
{
std::vector<std::string> robot_poses_json;
robot_poses_json.resize (robot_poses.size ());
for (uint i = 0; i < robot_poses_json.size (); ++i)
{
if (!matrixTransformationToJson (robot_poses[i], robot_poses_json[i]))
return (false);
}
// Feed all robot poses into the calibration command
NxLibCommand calibrate (cmdCalibrateHandEye);
for (uint i = 0; i < robot_poses_json.size (); ++i)
{
// Very weird behaviour here:
// If you modify this loop, check that all the transformations are still here in the [itmExecute][itmParameters] node
// because for an unknown reason sometimes the old transformations are erased in the tree ("null" in the tree)
calibrate.parameters ()[itmTransformations][i].setJson (robot_poses_json[i], false);
}
// Set Hand-Eye calibration parameters
if (boost::iequals (setup, "Fixed"))
calibrate.parameters ()[itmSetup].set (valFixed);
else
calibrate.parameters ()[itmSetup].set (valMoving);
calibrate.parameters ()[itmTarget].set (target);
// Set guess matrix
if (guess_tf.matrix () != Eigen::Matrix4d::Identity ())
{
// Matrix > JSON > Angle axis
NxLibItem tf ("/tmpTF");
if (!matrixTransformationToJson (guess_tf, json))
return (false);
tf.setJson (json);
// Rotation
double theta = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation
double x = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector
double y = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector
double z = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector
(*root_)[itmLink][itmRotation][itmAngle].set (theta);
(*root_)[itmLink][itmRotation][itmAxis][0].set (x);
(*root_)[itmLink][itmRotation][itmAxis][1].set (y);
(*root_)[itmLink][itmRotation][itmAxis][2].set (z);
// Translation
(*root_)[itmLink][itmTranslation][0].set (guess_tf.translation ()[0]);
(*root_)[itmLink][itmTranslation][1].set (guess_tf.translation ()[1]);
(*root_)[itmLink][itmTranslation][2].set (guess_tf.translation ()[2]);
}
calibrate.execute (); // Might take up to 120 sec (maximum allowed by Ensenso API)
if (calibrate.successful ())
{
json = calibrate.result ().asJson (pretty_format);
return (true);
}
else
return (false);
}
catch (NxLibException &ex)
{
ensensoExceptionHandling (ex, "computeCalibrationMatrix");
return (false);
}
}
bool Mapper::getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
{
if (!mapPointCloud)
return false;
const float max_x = req.topRightCorner.x;
const float max_y = req.topRightCorner.y;
const float max_z = req.topRightCorner.z;
const float min_x = req.bottomLeftCorner.x;
const float min_y = req.bottomLeftCorner.y;
const float min_z = req.bottomLeftCorner.z;
cerr << "min [" << min_x << ", " << min_y << ", " << min_z << "] " << endl;
cerr << "max [" << max_x << ", " << max_y << ", " << max_z << "] " << endl;
tf::StampedTransform stampedTr;
Eigen::Affine3d eigenTr;
tf::poseMsgToEigen(req.mapCenter, eigenTr);
Eigen::MatrixXf T = eigenTr.matrix().inverse().cast<float>();
//const Eigen::MatrixXf T = eigenTr.matrix().cast<float>();
cerr << "T:" << endl << T << endl;
T = transformation->correctParameters(T);
// FIXME: do we need a mutex here?
const DP centeredPointCloud = transformation->compute(*mapPointCloud, T);
DP cutPointCloud = centeredPointCloud.createSimilarEmpty();
cerr << centeredPointCloud.features.topLeftCorner(3, 10) << endl;
cerr << T << endl;
int newPtCount = 0;
for(int i=0; i < centeredPointCloud.features.cols(); i++)
{
const float x = centeredPointCloud.features(0,i);
const float y = centeredPointCloud.features(1,i);
const float z = centeredPointCloud.features(2,i);
if(x < max_x && x > min_x &&
y < max_y && y > min_y &&
z < max_z && z > min_z )
{
cutPointCloud.setColFrom(newPtCount, centeredPointCloud, i);
newPtCount++;
}
}
cerr << "Extract " << newPtCount << " points from the map" << endl;
cutPointCloud.conservativeResize(newPtCount);
cutPointCloud = transformation->compute(cutPointCloud, T.inverse());
// Send the resulting point cloud in ROS format
res.boundedMap = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cutPointCloud, mapFrame, ros::Time::now());
return true;
}
int main(int argc, char** argv)
{
// welcome message
cout << "****************************************************" << endl;
cout << "* Surface Matching demonstration : demonstrates the use of surface matching"
" using point pair features." << endl;
cout << "* The sample loads a model and a scene, where the model lies in a different"
" pose than the training.\n* It then trains the model and searches for it in the"
" input scene. The detected poses are further refined by ICP\n* and printed to the "
" standard output." << endl;
cout << "****************************************************" << endl;
if (argc < 3)
{
help("Not enough input arguments");
exit(1);
}
#if (defined __x86_64__ || defined _M_X64)
cout << "Running on 64 bits" << endl;
#else
cout << "Running on 32 bits" << endl;
#endif
#ifdef _OPENMP
cout << "Running with OpenMP" << endl;
#else
cout << "Running without OpenMP and without TBB" << endl;
#endif
string modelFileName = (string)argv[1];
string sceneFileName = (string)argv[2];
Mat pc = cv::ppf_match_3d::loadPLYSimple(modelFileName.c_str(), 1);
// Now train the model
cout << "Training..." << endl;
int64 tick1 = cv::getTickCount();
ppf_match_3d::PPF3DDetector detector(0.025, 0.05);
detector.trainModel(pc);
int64 tick2 = cv::getTickCount();
cout << endl << "Training complete in "
<< (double)(tick2-tick1)/ cv::getTickFrequency()
<< " sec" << endl << "Loading model..." << endl;
// Read the scene
Mat pcTest = cv::ppf_match_3d::loadPLYSimple(sceneFileName.c_str(), 1);
// Match the model to the scene and get the pose
cout << endl << "Starting matching..." << endl;
vector<cv::ppf_match_3d::Pose3DPtr> results;
tick1 = cv::getTickCount();
detector.match(pcTest, results, 1.0/40.0, 0.05);
tick2 = cv::getTickCount();
cout << endl << "PPF Elapsed Time " <<
(tick2-tick1)/cv::getTickFrequency() << " sec" << endl;
//check results size from match call above
size_t results_size = results.size();
cout << "Number of matching poses: " << results_size ;
if (results_size == 0) {
cout << endl << "No matching poses found. Exiting." << endl;
exit(0);
}
// Get only first N results - but adjust to results size if num of results are less than that specified by N
size_t N = 8;
if (results_size < N) {
cout << endl << "Reducing matching poses to be reported (as specified in code): "
<< N << " to the number of matches found: " << results_size << endl;
N = results_size;
}
vector<cv::ppf_match_3d::Pose3DPtr> resultsSub(results.begin(),results.begin()+N);
typedef double Scalar;
typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Vertices;
// Map the OpenCV matrix with Eigen:
Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> pcTestEigen(pcTest.ptr<float>(), pcTest.rows, pcTest.cols);
Eigen::MatrixXf eigenScene = pcTestEigen.transpose();
Vertices eigenScenePoints = eigenScene.topRows<3>().cast<Scalar>();
//ICP icp(100, 0.005f, 2.5f, 8);
int64 t1 = cv::getTickCount();
// Register for all selected poses
cout << endl << "Performing ICP on " << N << " poses..." << endl;
std::vector<Vertices> transformedModels;
for (auto& result: resultsSub)
{
cv::Mat pct = cv::ppf_match_3d::transformPCPose(pc, result->pose);
// Map the OpenCV matrix with Eigen:
Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> pcModelEigen(pct.ptr<float>(), pct.rows, pct.cols);
Vertices eigenModelPoints = pcModelEigen.transpose().topRows<3>().cast<double>();
Vertices eigenModelNormals = pcModelEigen.transpose().middleRows<3>(3).cast<double>();
//SICP::point_to_plane(eigenScenePoints, eigenModelPoints, eigenModelNormals);
SICP::point_to_point(eigenModelPoints, eigenScenePoints);
transformedModels.push_back(eigenModelPoints);
}
//icp.registerModelToScene(pc, pcTest, resultsSub);
int64 t2 = cv::getTickCount();
cout << endl << "ICP Elapsed Time " <<
(t2-t1)/cv::getTickFrequency() << " sec" << endl;
cout << "Poses: " << endl;
Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> pcModelEigen(pc.ptr<float>(), pc.rows, pc.cols);
Vertices eigenModelPoints = pcModelEigen.transpose().topRows<3>().cast<double>();
Vertices eigenModelNormals = pcModelEigen.transpose().middleRows<3>(3).cast<double>();
bool saved = false;
// debug first five poses
for (auto& transformedModel : transformedModels)
{
Eigen::Affine3d transform = RigidMotionEstimator::point_to_point(eigenModelPoints, transformedModel);
cout << "\nPose Result:\n " << transform.matrix() << endl;
if (!saved)
{
Eigen::MatrixXf floatModel = transformedModel.transpose().cast<float>();
cv::Mat cvTransformedModel;
eigen2cv(floatModel, cvTransformedModel);
cv::ppf_match_3d::writePLY(cvTransformedModel, "transformedModelOut.ply");
}
}
return 0;
}
bool CloudMatcher::matchWithGuess(ethzasl_icp_mapper::MatchCloudsWithGuess::Request& req, ethzasl_icp_mapper::MatchCloudsWithGuess::Response& res)
{
// get and check reference
const DP referenceCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.reference));
const unsigned referenceGoodCount(referenceCloud.features.cols());
const unsigned referencePointCount(req.reference.width * req.reference.height);
const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
if (referenceGoodCount == 0)
{
ROS_ERROR("I found no good points in the reference cloud");
return false;
}
if (referenceGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
}
// get and check reading
const DP readingCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.readings));
const unsigned readingGoodCount(referenceCloud.features.cols());
const unsigned readingPointCount(req.readings.width * req.readings.height);
const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
if (readingGoodCount == 0)
{
ROS_ERROR("I found no good points in the reading cloud");
return false;
}
if (readingGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
}
// check dimensions
if (referenceCloud.features.rows() != readingCloud.features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: reference cloud is " << referenceCloud.features.rows()-1 << " while reading cloud is " << readingCloud.features.rows()-1);
return false;
}
//PointMatcher type for Initial Guess
PM::TransformationParameters TInitialGuess;
tf::Transform transformInitialGuess;//Create a Transform Object
//tf::transformMsgToTF(req.initialGuess, transformInitialGuess);//Convert ROS Transform msg to tf data
//Eigen::Matrix4f //Creat a Eigen Object
Eigen::Affine3d initialGuessInEigenMatrix; //Eigen Object
tf::transformMsgToEigen(req.initialGuess, initialGuessInEigenMatrix);
//TInitialGuess = initialGuessInEigenMatrix.matrix().cast<float>();
TInitialGuess = initialGuessInEigenMatrix.matrix().cast<float>();
// call ICP
try
{
PM::TransformationParameters Ticp;
Ticp = icp(readingCloud, referenceCloud, TInitialGuess);
geometry_msgs::Transform TicpMsg;
//const PM::TransformationParameters transform(icp(readingCloud, referenceCloud, TInitialGuess));
tf::transformTFToMsg(PointMatcher_ros::eigenMatrixToTransform<float>(Ticp), TicpMsg);
res.transform = TicpMsg;
ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return false;
}
return true;
}
bool
pcl::EnsensoGrabber::computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
std::string &json,
const std::string setup,
const std::string target,
const Eigen::Affine3d &guess_tf,
const bool pretty_format) const
{
if ( (*root_)[itmVersion][itmMajor] < 2 && (*root_)[itmVersion][itmMinor] < 3)
PCL_WARN ("EnsensoSDK 1.3.x fixes bugs into extrinsic calibration matrix optimization, please update your SDK!\n"
"http://www.ensenso.de/support/sdk-download/\n");
try
{
std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > robot_poses_mm (robot_poses);
std::vector<std::string> robot_poses_json;
robot_poses_json.resize (robot_poses.size ());
for (size_t i = 0; i < robot_poses_json.size (); ++i)
{
robot_poses_mm[i].translation () *= 1000.0; // Convert meters in millimeters
if (!matrixTransformationToJson (robot_poses_mm[i], robot_poses_json[i]))
return (false);
}
NxLibCommand calibrate (cmdCalibrateHandEye);
// Set Hand-Eye calibration parameters
if (boost::iequals (setup, "Fixed"))
calibrate.parameters ()[itmSetup].set (valFixed);
else
calibrate.parameters ()[itmSetup].set (valMoving);
calibrate.parameters ()[itmTarget].set (target);
// Set guess matrix
if (guess_tf.matrix () != Eigen::Matrix4d::Identity ())
{
// Matrix > JSON > Angle axis
NxLibItem tf ("/tmpTF");
if (!matrixTransformationToJson (guess_tf, json))
return (false);
tf.setJson (json);
// Rotation
double theta = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation
double x = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector
double y = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector
double z = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector
tf.erase(); // Delete tmpTF node
calibrate.parameters ()[itmLink][itmRotation][itmAngle].set (theta);
calibrate.parameters ()[itmLink][itmRotation][itmAxis][0].set (x);
calibrate.parameters ()[itmLink][itmRotation][itmAxis][1].set (y);
calibrate.parameters ()[itmLink][itmRotation][itmAxis][2].set (z);
// Translation
calibrate.parameters ()[itmLink][itmTranslation][0].set (guess_tf.translation ()[0] * 1000.0);
calibrate.parameters ()[itmLink][itmTranslation][1].set (guess_tf.translation ()[1] * 1000.0);
calibrate.parameters ()[itmLink][itmTranslation][2].set (guess_tf.translation ()[2] * 1000.0);
}
// Feed all robot poses into the calibration command
for (size_t i = 0; i < robot_poses_json.size (); ++i)
{
// Very weird behavior here:
// If you modify this loop, check that all the transformations are still here in the [itmExecute][itmParameters] node
// because for an unknown reason sometimes the old transformations are erased in the tree ("null" in the tree)
// Ensenso SDK 2.3.348: If not moved after guess calibration matrix, the vector is empty.
calibrate.parameters ()[itmTransformations][i].setJson (robot_poses_json[i], false);
}
calibrate.execute (); // Might take up to 120 sec (maximum allowed by Ensenso API)
if (calibrate.successful ())
{
json = calibrate.result ().asJson (pretty_format);
return (true);
}
else
{
json.clear ();
return (false);
}
}
catch (NxLibException &ex)
{
ensensoExceptionHandling (ex, "computeCalibrationMatrix");
return (false);
}
}
cloud_normal_t::Ptr
read_scan_with_normals(e57::Reader& reader, uint32_t scan_index,
const Eigen::Vector3d& demean_offset) {
e57::Data3D header;
reader.ReadData3D(scan_index, header);
int64_t nColumn = 0, nRow = 0, nPointsSize = 0, nGroupsSize = 0,
nCounts = 0;
bool bColumnIndex = 0;
reader.GetData3DSizes(scan_index, nRow, nColumn, nPointsSize, nGroupsSize,
nCounts, bColumnIndex);
int64_t n_size = (nRow > 0) ? nRow : 1024;
double* data_x = new double[n_size], * data_y = new double[n_size],
* data_z = new double[n_size];
double* nrm_x = new double[n_size], * nrm_y = new double[n_size],
* nrm_z = new double[n_size];
int8_t valid_normals = 0;
auto block_read = reader.SetUpData3DPointsData(
scan_index, n_size, data_x, data_y, data_z, NULL, NULL, NULL, NULL,
NULL, NULL, NULL, nrm_x, nrm_y, nrm_z, &valid_normals);
Eigen::Affine3d rotation;
rotation =
Eigen::Quaterniond(header.pose.rotation.w, header.pose.rotation.x,
header.pose.rotation.y, header.pose.rotation.z);
Eigen::Affine3d registration;
registration = Eigen::Translation<double, 3>(
header.pose.translation.x + demean_offset[0],
header.pose.translation.y + demean_offset[1],
header.pose.translation.z + demean_offset[2]) *
rotation;
Eigen::Affine3d normal_transformation;
normal_transformation = rotation.matrix().inverse().transpose();
unsigned long size = 0;
cloud_normal_t::Ptr scan(new cloud_normal_t());
while ((size = block_read.read()) > 0) {
for (unsigned long i = 0; i < size; i++) {
point_normal_t p;
Eigen::Vector3d pos(data_x[i], data_y[i], data_z[i]);
pos = registration * pos;
p.x = static_cast<float>(pos[0]);
p.y = static_cast<float>(pos[1]);
p.z = static_cast<float>(pos[2]);
Eigen::Vector3d nrm(nrm_x[i], nrm_y[i], nrm_z[i]);
nrm = normal_transformation * nrm;
p.normal[0] = static_cast<float>(nrm[0]);
p.normal[1] = static_cast<float>(nrm[1]);
p.normal[2] = static_cast<float>(nrm[2]);
scan->push_back(p);
}
}
block_read.close();
delete[] data_x;
delete[] data_y;
delete[] data_z;
delete[] nrm_x;
delete[] nrm_y;
delete[] nrm_z;
// get origin position
scan->sensor_origin_ =
Eigen::Vector4f(header.pose.translation.x + demean_offset[0],
header.pose.translation.y + demean_offset[1],
header.pose.translation.z + demean_offset[2], 1.0);
return scan;
}
// Usage: ./Volumetricd.exe ../../data/monkey.obj 256 4 2 90
int main(int argc, char **argv)
{
if (argc < 6)
{
std::cerr << "Missing parameters. Abort."
<< std::endl
<< "Usage: ./Volumetricd.exe ../../data/monkey.obj 256 8 2 90"
<< std::endl;
return EXIT_FAILURE;
}
Timer timer;
const std::string filepath = argv[1];
const int vol_size = atoi(argv[2]);
const int vx_size = atoi(argv[3]);
const int cloud_count = atoi(argv[4]);
const int rot_interval = atoi(argv[5]);
std::pair<std::vector<double>, std::vector<double>> depth_buffer;
//
// Projection and Modelview Matrices
//
Eigen::Matrix4d K = perspective_matrix(fov_y, aspect_ratio, near_plane, far_plane);
std::pair<Eigen::Matrix4d, Eigen::Matrix4d> T(Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity());
//
// Creating volume
//
Eigen::Vector3d voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3d volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3d voxel_count(volume_size.x() / voxel_size.x(), volume_size.y() / voxel_size.y(), volume_size.z() / voxel_size.z());
//
Eigen::Affine3d grid_affine = Eigen::Affine3d::Identity();
grid_affine.translate(Eigen::Vector3d(0, 0, -256));
grid_affine.scale(Eigen::Vector3d(1, 1, -1)); // z is negative inside of screen
Grid grid(volume_size, voxel_size, grid_affine.matrix());
//
// Importing .obj
//
timer.start();
std::vector<Eigen::Vector3d> points3DOrig, pointsTmp;
import_obj(filepath, points3DOrig);
timer.print_interval("Importing monkey : ");
std::cout << "Monkey point count : " << points3DOrig.size() << std::endl;
//
// Translating and rotating monkey point cloud
std::pair<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>> cloud;
//
Eigen::Affine3d rotate = Eigen::Affine3d::Identity();
Eigen::Affine3d translate = Eigen::Affine3d::Identity();
translate.translate(Eigen::Vector3d(0, 0, -256));
//
// Compute first cloud
//
for (Eigen::Vector3d p3d : points3DOrig)
{
Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
rot /= rot.w();
cloud.first.push_back(rot.head<3>());
}
//
// Update grid with first cloud
//
timer.start();
create_depth_buffer<double>(depth_buffer.first, cloud.first, K, Eigen::Matrix4d::Identity(), far_plane);
timer.print_interval("CPU compute depth : ");
timer.start();
update_volume(grid, depth_buffer.first, K, T.first);
timer.print_interval("CPU Update volume : ");
//
// Compute next clouds
Eigen::Matrix4d cloud_mat = Eigen::Matrix4d::Identity();
Timer iter_timer;
for (int i = 1; i < cloud_count; ++i)
{
std::cout << std::endl << i << " : " << i * rot_interval << std::endl;
iter_timer.start();
// Rotation matrix
rotate = Eigen::Affine3d::Identity();
rotate.rotate(Eigen::AngleAxisd(DegToRad(i * rot_interval), Eigen::Vector3d::UnitY()));
cloud.second.clear();
for (Eigen::Vector3d p3d : points3DOrig)
{
Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
rot /= rot.w();
cloud.second.push_back(rot.head<3>());
}
//export_obj("../../data/cloud_cpu_2.obj", cloud.second);
timer.start();
create_depth_buffer<double>(depth_buffer.second, cloud.second, K, Eigen::Matrix4d::Identity(), far_plane);
timer.print_interval("Compute depth buffer: ");
//export_depth_buffer("../../data/cpu_depth_buffer_2.obj", depth_buffer.second);
timer.start();
Eigen::Matrix4d icp_mat;
ComputeRigidTransform(cloud.first, cloud.second, icp_mat);
timer.print_interval("Compute rigid transf: ");
//std::cout << std::fixed << std::endl << "icp_mat " << std::endl << icp_mat << std::endl;
// accumulate matrix
cloud_mat = cloud_mat * icp_mat;
//std::cout << std::fixed << std::endl << "cloud_mat " << std::endl << cloud_mat << std::endl;
timer.start();
//update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
timer.print_interval("Update volume : ");
// copy second point cloud to first
cloud.first = cloud.second;
//depth_buffer.first = depth_buffer.second;
iter_timer.print_interval("Iteration time : ");
}
//std::cout << "------- // --------" << std::endl;
//for (int i = 0; i < grid.data.size(); ++i)
//{
// const Eigen::Vector3d& point = grid.data[i].point;
// std::cout << point.transpose() << "\t\t" << grid.data[i].tsdf << " " << grid.data[i].weight << std::endl;
//}
//std::cout << "------- // --------" << std::endl;
// timer.start();
// export_volume("../../data/grid_volume_cpu.obj", grid.data);
// timer.print_interval("Exporting volume : ");
// return 0;
QApplication app(argc, argv);
//
// setup opengl viewer
//
GLModelViewer glwidget;
glwidget.resize(640, 480);
glwidget.setPerspective(60.0f, 0.1f, 10240.0f);
glwidget.move(320, 0);
glwidget.setWindowTitle("Point Cloud");
glwidget.setWeelSpeed(0.1f);
glwidget.setDistance(-0.5f);
glwidget.show();
Eigen::Matrix4d to_origin = Eigen::Matrix4d::Identity();
to_origin.col(3) << -(volume_size.x() / 2.0), -(volume_size.y() / 2.0), -(volume_size.z() / 2.0), 1.0; // set translate
std::vector<Eigen::Vector4f> vertices, colors;
int i = 0;
for (int z = 0; z <= volume_size.z(); z += voxel_size.z())
{
for (int y = 0; y <= volume_size.y(); y += voxel_size.y())
{
for (int x = 0; x <= volume_size.x(); x += voxel_size.x(), i++)
{
const float tsdf = grid.data.at(i).tsdf;
//Eigen::Vector4d p = grid_affine.matrix() * to_origin * Eigen::Vector4d(x, y, z, 1);
Eigen::Vector4d p = to_origin * Eigen::Vector4d(x, y, z, 1);
p /= p.w();
if (tsdf > 0.1)
{
vertices.push_back(p.cast<float>());
colors.push_back(Eigen::Vector4f(0, 1, 0, 1));
}
else if (tsdf < -0.1)
{
vertices.push_back(p.cast<float>());
colors.push_back(Eigen::Vector4f(1, 0, 0, 1));
}
}
}
}
//
// setup model
//
std::shared_ptr<GLModel> model(new GLModel);
model->initGL();
model->setVertices(&vertices[0][0], vertices.size(), 4);
model->setColors(&colors[0][0], colors.size(), 4);
glwidget.addModel(model);
//
// setup kinect shader program
//
std::shared_ptr<GLShaderProgram> kinectShaderProgram(new GLShaderProgram);
if (kinectShaderProgram->build("color.vert", "color.frag"))
model->setShaderProgram(kinectShaderProgram);
return app.exec();
}