void X3DConverter::startShape(const std::array<float, 12>& matrix) {
// Finding axis/angle from matrix using Eigen for its bullet proof implementation.
Eigen::Transform<float, 3, Eigen::Affine> t;
t.setIdentity();
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = 0; j < 4; j++) {
t(i, j) = matrix[i+j*3];
}
}
Eigen::Matrix3f rotationMatrix;
Eigen::Matrix3f scaleMatrix;
t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
Eigen::Quaternionf q;
Eigen::AngleAxisf aa;
q = rotationMatrix;
aa = q;
Eigen::Vector3f scale = scaleMatrix.diagonal();
Eigen::Vector3f translation = t.translation();
startNode(ID::Transform);
m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
startNode(ID::Shape);
startNode(ID::Appearance);
startNode(ID::Material);
m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
endNode(ID::Material); // Material
endNode(ID::Appearance); // Appearance
}
TEST(DOF6, Source)
//void t1()
{
time_t ti = time(NULL);
ROS_INFO("init Source with %d",(int)ti);
srand(ti);
for(int i=0; i<CYCLES; i++) {
DOF6::TFLinkvf rot1, rot2;
const Eigen::AngleAxisf aa=createRandomAA();
const Eigen::Vector3f t=createRandomT();
//tf1 should be tf2
Eigen::Matrix4f tf1 = build_random_tflink(rot1,30,0.4,aa,t);
Eigen::Matrix4f tf2 = build_random_tflink(rot2,30,0.2,aa,t);
//check
const float d1=MATRIX_DISTANCE(rot1.getTransformation(),tf1,0.4);
const float d2=MATRIX_DISTANCE(rot2.getTransformation(),tf2,0.2);
DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::TFLinkvf> abc(rot1.makeShared(), rot2.makeShared());
// std::cout<<"rot\n"<<aa.toRotationMatrix()<<"\n";
// std::cout<<"t\n"<<t<<"\n";
//
// std::cout<<"rot\n"<<rot1.getRotation()<<"\n";
// std::cout<<"t\n"<<rot1.getTranslation()<<"\n";
//
// std::cout<<"rot\n"<<rot2.getRotation()<<"\n";
// std::cout<<"t\n"<<rot2.getTranslation()<<"\n";
//
// std::cout<<"rot\n"<<abc.getRotation().toRotMat()<<"\n";
// std::cout<<"t\n"<<abc.getTranslation()<<"\n";
//
//
// std::cout<<"getRotationVariance "<<rot1.getRotationVariance()<<"\n";
// std::cout<<"getTranslationVariance "<<rot1.getTranslationVariance()<<"\n";
//
// std::cout<<"getRotationVariance "<<rot2.getRotationVariance()<<"\n";
// std::cout<<"getTranslationVariance "<<rot2.getTranslationVariance()<<"\n";
//
// std::cout<<"getRotationVariance "<<abc.getRotationVariance()<<"\n";
// std::cout<<"getTranslationVariance "<<abc.getTranslationVariance()<<"\n";
float d3=MATRIX_DISTANCE((Eigen::Matrix3f)abc.getRotation(),aa.toRotationMatrix(),0.2);
EXPECT_NEAR((abc.getTranslation()-t).norm(),0,0.2);
d3+=(abc.getTranslation()-t).norm();
//EXPECT_LE(d3,std::max(d1,d2));
}
}
gsl_vector* VelStereoMatcher::stereoToVec(const StereoProperties stereo)
{
Eigen::Affine3f tform = stereo.getLeftCamera().tform;
Eigen::Matrix4f intrinsics = stereo.getLeftCamera().intrinsics.matrix();
Eigen::Vector3f tran;
tran = tform.translation();
float x = tran[0];
float y = tran[1];
float z = tran[2];
Eigen::Matrix3f mat = tform.rotation();
Eigen::AngleAxisf axis;
axis = mat;
float ax = axis.axis()[0] * axis.angle();
float ay = axis.axis()[1] * axis.angle();
float az = axis.axis()[2] * axis.angle();
float fx = intrinsics(0,0);
float fy = intrinsics(1,1);
float cx = intrinsics(0,2);
float cy = intrinsics(1,2);
float baseline = stereo.baseline;
gsl_vector* vec = gsl_vector_alloc(11);
gsl_vector_set(vec, 0, x);
gsl_vector_set(vec, 1, y);
gsl_vector_set(vec, 2, z);
gsl_vector_set(vec, 3, ax);
gsl_vector_set(vec, 4, ay);
gsl_vector_set(vec, 5, az);
gsl_vector_set(vec, 6, fx);
gsl_vector_set(vec, 7, fy);
gsl_vector_set(vec, 8, cx);
gsl_vector_set(vec, 9, cy);
gsl_vector_set(vec, 10, baseline);
return vec;
}
vtkSmartPointer<vtkDataSet>
pcl::visualization::createCube (const pcl::ModelCoefficients &coefficients)
{
// coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
Eigen::AngleAxisf a (Eigen::Quaternionf (coefficients.values[6], coefficients.values[3],
coefficients.values[4], coefficients.values[5]));
t->RotateWXYZ (pcl::rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetXLength (coefficients.values[7]);
cube->SetYLength (coefficients.values[8]);
cube->SetZLength (coefficients.values[9]);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (cube->GetOutputPort ());
return (tf->GetOutput ());
}
vtkSmartPointer<vtkDataSet>
pcl::visualization::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation,
double width, double height, double depth)
{
// coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (translation.x (), translation.y (), translation.z ());
Eigen::AngleAxisf a (rotation);
t->RotateWXYZ (pcl::rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetXLength (width);
cube->SetYLength (height);
cube->SetZLength (depth);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (cube->GetOutputPort ());
return (tf->GetOutput ());
}
Eigen::Matrix4f build_random_tflink(DOF6::TFLinkvf &tflink, const int N=2, const float noise=0.f, const Eigen::AngleAxisf aa=createRandomAA(), const Eigen::Vector3f t=createRandomT()) {
Eigen::Vector3f n, v,n2;
v(0)=0.01f;v(2)=v(1)=1.f; //"some" plane
//std::cout<<"t\n"<<t<<"\n";
std::vector<Eigen::Vector3f> normal;
std::vector<Eigen::Vector3f> normal2;
for(int j=0; j<std::max(2,N); j++) {
//seconds
n(0) = (rand()%1000)/1000.f-0.5f; //init normals
n(1) = (rand()%1000)/1000.f-0.5f;
n(2) = (rand()%1000)/1000.f-0.5f;
n.normalize();
n2 = aa.toRotationMatrix()*n;
n2(0) += 2*noise*((rand()%1000)/1000.f-0.5f); //add noise
n2(1) += 2*noise*((rand()%1000)/1000.f-0.5f);
n2(2) += 2*noise*((rand()%1000)/1000.f-0.5f);
n2.normalize();
normal.push_back(n);
normal2.push_back(n2);
}
pcl::TransformationFromCorrespondences tfc;
for(size_t i=0; i<normal.size(); i++) {
//std::cout<<"normal\n"<<normal2[i]<<"\n";
//std::cout<<"t\n"<<(normal2[i].dot(t)*normal2[i])<<"\n";
int s=rand()%3;
float w = (rand()%100+1)/10.f;
if((s==0 && normal2[i].dot(t)>0)) //plane
{
std::cout<<"PLANE\n";
tflink(DOF6::TFLinkvf::TFLinkObj(normal[i],true,false,w),
DOF6::TFLinkvf::TFLinkObj(normal2[i]*(1+(normal2[i].dot(t)))
,true,false,w));
}
else if((s==1 && normal2[i].dot(t)>0)) //plane
{
std::cout<<"NORMAL\n";
tflink(DOF6::TFLinkvf::TFLinkObj(normal[i], true,true,w),
DOF6::TFLinkvf::TFLinkObj(normal2[i],true,true,w));
}
else
{
std::cout<<"CUBE\n";
tflink(DOF6::TFLinkvf::TFLinkObj(normal[i],false,false,w),
DOF6::TFLinkvf::TFLinkObj(normal2[i]+t,false,false,w));
}
tfc.add(normal[i],normal2[i]+t); //always with t
}
tflink.finish();
Eigen::Matrix4f r=Eigen::Matrix4f::Identity();
for(int i=0; i<3; i++) {
for(int j=0; j<3; j++)
r(i,j) = aa.toRotationMatrix()(i,j);
r.col(3)(i)= t(i);
}
static float dis2=0,dis3=0;
float d2=0,d3=0;
d2=MATRIX_DISTANCE(tflink.getTransformation(),r,10000);
d3=MATRIX_DISTANCE(tfc.getTransformation().matrix(),r,10000);
dis2+=d2;
dis3+=d3;
std::cout<<tflink<<"\n";
std::cout<<"tfl: "<<d2<<"\n";
std::cout<<"tfc: "<<d3<<"\n";
std::cout<<"dis2: "<<dis2<<"\n";
std::cout<<"dis3: "<<dis3<<"\n";
return r;
}
//tracker step
void Tracker::track()
{
storage->readSceneProcessed(scene);
if (error_count >= 30){
//failed 30 times in a row
ROS_ERROR("[Tracker::%s] Object is lost ... stopping tracker...",__func__);
started = false;
lost_it = true;
return;
}
PXC::Ptr target (new PXC);
crop_a_box(scene, target, (*bounding_box)*factor, false, *transform, false);
if (target->points.size() <= 15){
ROS_ERROR_THROTTLE(10,"[Tracker::%s] Not enought points in bounding box, retryng with larger bounding box", __func__);
factor += 0.2;
rej_distance +=0.005;
++error_count;
return;
}
/*
* Alignment
*/
//check if user changed leaf size
double val;
PXC::Ptr aligned = boost::make_shared<PXC>();
basic_config->get("downsampling_leaf_size", val);
if (val != leaf){
leaf = val;
computeModel();
icp.setInputSource(model);
pcl::CentroidPoint<PX> mc;
for (size_t i=0; i<model->points.size(); ++i)
mc.add(model->points[i]);
mc.get(model_centroid);
}
// bool feat_align(true);
// if (feat_align){
// NTC::Ptr target_n = boost::make_shared<NTC>();
// pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_f = boost::make_shared<pcl::PointCloud<pcl::FPFHSignature33>>();
// ne.setRadiusSearch(2.0f*leaf);
// ne.useSensorOriginAsViewPoint();
// ne.setInputCloud(target);
// ne.compute(*target_n);
// fpfh.setInputNormals(target_n);
// fpfh.setInputCloud(target);
// fpfh.setRadiusSearch(3.5f*leaf);
// fpfh.compute(*target_f);
// //Assemble correspondences based on model-target features
// SearchT tree (true, CreatorT(new IndexT(4)));
// tree.setPointRepresentation (RepT(new pcl::DefaultFeatureRepresentation<pcl::FPFHSignature33>));
// tree.setChecks(256);
// tree.setInputCloud(target_f);
// //Search model features over target features
// //If model features are n, these will be n*k_nn matrices
// std::vector<std::vector<int>> k_idx;
// std::vector<std::vector<float>> k_dist;
// int k_nn(1);
// tree.nearestKSearch (*model_feat, std::vector<int> (), k_nn, k_idx, k_dist);
// //define a distance threshold
// float dist_thresh_m = 125.0f;
// //fill in model-target correpsondences
// pcl::Correspondences corr_m_over_t;
// for(size_t i=0; i < k_idx.size(); ++i)
// {
// if (k_dist[i][0] > dist_thresh_m){
// //we have a correspondence
// PX p1 (model->points[i]);
// PX p2 (target->points[k_idx[i][0]]);
// Eigen::Vector3f diff (p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
// float eu_dist = diff.squaredNorm();
// //Add a correspondence only if distance is below threshold
// pcl::Correspondence cor(i, k_idx[i][0], eu_dist);
// corr_m_over_t.push_back(cor);
// }
// }
// //Estimate the rigid transformation of model -> target
// teDQ->estimateRigidTransformation(*model, *target, corr_m_over_t, *transform);
// }
crd->setMaximumDistance(rej_distance);
icp.setInputTarget(target);
if (centroid_counter >=10){
pcl::CentroidPoint<PX> tc;
for (size_t i=0; i<target->points.size(); ++i)
tc.add(target->points[i]);
PX target_centroid, mc_transformed;
mc_transformed = pcl::transformPoint(model_centroid, Eigen::Affine3f(*transform));
tc.get(target_centroid);
Eigen::Matrix4f Tcen, guess;
Tcen << 1, 0, 0, (target_centroid.x - mc_transformed.x),
0, 1, 0, (target_centroid.y - mc_transformed.y),
0, 0, 1, (target_centroid.z - mc_transformed.z),
0, 0, 0, 1;
guess = Tcen*(*transform);
icp.align(*aligned, guess);
centroid_counter = 0;
ROS_WARN("[Tracker::%s] Centroid Translation Performed!",__func__);
centroid_counter = 0;
}
else if (disturbance_counter >= 20)
{
float angx = D2R*UniformRealIn(30.0,90.0,true);
float angy = D2R*UniformRealIn(30.0,90.0,true);
float angz = D2R*UniformRealIn(30.0,90.0,true);
Eigen::Matrix4f T_rotx, T_roty, T_rotz;
if (UniformIntIn(0,1))
angx *= -1;
if (UniformIntIn(0,1))
angy *= -1;
if (UniformIntIn(0,1))
angz *= -1;
Eigen::AngleAxisf rotx (angx, Eigen::Vector3f::UnitX());
T_rotx<< rotx.matrix()(0,0), rotx.matrix()(0,1), rotx.matrix()(0,2), 0,
rotx.matrix()(1,0), rotx.matrix()(1,1), rotx.matrix()(1,2), 0,
rotx.matrix()(2,0), rotx.matrix()(2,1), rotx.matrix()(2,2), 0,
0, 0, 0, 1;
Eigen::AngleAxisf roty (angy, Eigen::Vector3f::UnitY());
T_roty<< roty.matrix()(0,0), roty.matrix()(0,1), roty.matrix()(0,2), 0,
roty.matrix()(1,0), roty.matrix()(1,1), roty.matrix()(1,2), 0,
roty.matrix()(2,0), roty.matrix()(2,1), roty.matrix()(2,2), 0,
0, 0, 0, 1;
Eigen::AngleAxisf rotz (angz, Eigen::Vector3f::UnitZ());
T_rotz<< rotz.matrix()(0,0), rotz.matrix()(0,1), rotz.matrix()(0,2), 0,
rotz.matrix()(1,0), rotz.matrix()(1,1), rotz.matrix()(1,2), 0,
rotz.matrix()(2,0), rotz.matrix()(2,1), rotz.matrix()(2,2), 0,
0, 0, 0, 1;
Eigen::Matrix4f disturbed, inverse;
inverse = transform->inverse();
disturbed = (T_rotz*T_roty*T_rotx*inverse).inverse();
ROS_WARN("[Tracker::%s] Triggered Disturbance! With angles %g, %g, %g",__func__, angx*R2D, angy*R2D, angz*R2D);
icp.align(*aligned, disturbed);
disturbance_counter = 0;
}
else
icp.align(*aligned, *transform);
fitness = icp.getFitnessScore();
// ROS_WARN("Fitness %g", fitness);
*(transform) = icp.getFinalTransformation();
//adjust distance and factor according to fitness
if (fitness > 0.001 ){
//fitness is high something is prolly wrong
rej_distance +=0.001;
factor += 0.05;
if (rej_distance > 2.0)
rej_distance = 2.0;
if (factor > 5.0)
factor = 5.0;
++disturbance_counter;
++centroid_counter;
}
else if (fitness < 0.0006){
//all looks good
rej_distance -=0.005;
if(rej_distance < 0.015)
rej_distance = 0.015; //we dont want to go lower than this
factor -=0.05;
if(factor < 1.1)
factor = 1.1;
error_count = 0;
disturbance_counter = 0;
centroid_counter = 0;
}
}
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
std::ostringstream s;
s << "Feedback from marker '" << feedback->marker_name << "' "
<< " / control '" << feedback->control_name << "'";
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
mouseInUse = true;
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
{
// Compute FK for end effectors that have changed
// Call IK to get the joint states
moveit_msgs::GetPositionFKRequest req;
req.fk_link_names.push_back("RightArm");
req.robot_state.joint_state = planState;
req.header.stamp = ros::Time::now();
moveit_msgs::GetPositionFKResponse resp;
gFKinClient.call(req, resp);
std::cerr << "Response: " << resp.pose_stamped[0] << std::endl;
if (resp.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
{
joyInt.currentPose = resp.pose_stamped[0];
}
else
{
ROS_ERROR_STREAM("Failed to solve FK: " << resp.error_code.val);
}
gRPosePublisher.publish(joyInt.currentPose);
mouseInUse = false;
break;
}
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
{
Eigen::AngleAxisf aa;
aa = Eigen::Quaternionf(feedback->pose.orientation.w,
feedback->pose.orientation.x,
feedback->pose.orientation.y,
feedback->pose.orientation.z);
boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(feedback->marker_name);
Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
float angle;
// Get sign of angle
if (aa.axis().dot(axisVector) < 0)
{
angle = -aa.angle();
}
else
{
angle = aa.angle();
}
// Trim angle to joint limits
if (angle > targetJoint->limits->upper)
{
angle = targetJoint->limits->upper;
}
else if (angle < targetJoint->limits->lower)
{
angle = targetJoint->limits->lower;
}
// Locate the index of the solution joint in the plan state
for (int j = 0; j < planState.name.size(); j++)
{
if (feedback->marker_name == planState.name[j])
{
planState.position[j] = angle;
}
}
prevAA = aa;
// Time and Frame stamps
planState.header.frame_id = "/Body_TSY";
planState.header.stamp = ros::Time::now();
gStatePublisher.publish(planState);
break;
}
}
gIntServer->applyChanges();
}
