void MainWindow::on_pb_loadcloud_clicked()
{
cv::FileStorage store("/home/sy/set/data/savingcoud.yml", FileStorage::READ);
for(int i = 0; i < filelist.size(); i++)
{
Mat tmp_mat;
store["frame" + QString::number(i).toStdString()] >> tmp_mat;
double* data = reinterpret_cast<double*>(tmp_mat.data);
cv::Matx34d rotation_vector(data);//make Matx33d
save_camera.push_back(rotation_vector);
}
cout << save_camera.size() << endl;
cv::FileNode n1 = store["points"];
cv::FileNode n2 = store["rgb"];
cv::read(n1,save_point);
cv::read(n2,save_rgb);
cout << save_point.size() << endl;
cout << save_rgb.size() << endl;
store.release();
ui->widget->update(save_point,
save_rgb,
save_camera);
}
bool symmetry_detection_3d::run(viennamesh::algorithm_handle &)
{
//
// std::cout << "dynamic: " << jacobi_polynom<double>(4,2,2) << std::endl;
// std::cout << "static: " << static_jacobi_polynom<double,4>(2,2) << std::endl;
//
// return true;
mesh_handle input_mesh = get_required_input<mesh_handle>("mesh");
int geometric_dimension = viennagrid::geometric_dimension( input_mesh() );
int cell_dimension = viennagrid::cell_dimension( input_mesh() );
data_handle<int> p = get_required_input<int>("p");
data_handle<double> relative_integrate_tolerance = get_required_input<double>("relative_integrate_tolerance");
data_handle<double> absolute_integrate_tolerance = get_required_input<double>("absolute_integrate_tolerance");
// data_handle<int> max_iteration_count = get_required_input<int>("max_iteration_count");
data_handle<double> mirror_symmetry_tolerance = get_required_input<double>("mirror_symmetry_tolerance");
data_handle<double> rotational_symmetry_tolerance = get_required_input<double>("rotational_symmetry_tolerance");
if (geometric_dimension != 3)
return false;
if (cell_dimension != 2)
return false;
typedef viennagrid::mesh MeshType;
typedef point PointType;
typedef viennagrid::result_of::const_vertex_range<MeshType>::type ConstVertexRangeType;
typedef viennagrid::result_of::iterator<ConstVertexRangeType>::type ConstVertexRangeIterator;
double max_size = 0.0;
{
ConstVertexRangeType vertices(input_mesh());
for (ConstVertexRangeIterator vit = vertices.begin(); vit != vertices.end(); ++vit)
{
double cur_size = viennagrid::norm_2( viennagrid::get_point(*vit) );
if (cur_size > max_size)
max_size = cur_size;
}
}
info(1) << "Before start" << std::endl;
MeshType mesh;
viennagrid::copy( input_mesh(), mesh );
viennagrid::scale( mesh, 1.0/max_size );
info(1) << "After copy/scale" << std::endl;
viennautils::Timer timer;
timer.start();
RealGeneralizedMoment m_real(2*p(), mesh);
// , relative_integrate_tolerance(), absolute_integrate_tolerance(), max_iteration_count());
info(1) << "After calculating generalized moment (!!! took " << timer.get() << "sec !!!)" << std::endl;
double sphere_radius = 1.0;
if (get_input<double>("sphere_radius").valid())
sphere_radius = get_input<double>("sphere_radius")();
MeshType sphere;
viennagrid::make_sphere_hull( sphere, viennagrid::make_point(0,0,0), sphere_radius, 4 );
viennagrid::quantity_field gradient_field_real(0, 1);
gradient_field_real.set_name("gradient_real");
ConstVertexRangeType vertices(sphere);
for (ConstVertexRangeIterator vit = vertices.begin(); vit != vertices.end(); ++vit)
{
PointType const & pt = viennagrid::get_point(*vit);
double theta;
double phi;
double r;
to_spherical(pt, theta, phi, r);
double grad_real = m_real.grad(theta, phi, 1e-2);
gradient_field_real.set(*vit, grad_real);
}
// {
// int bench_count = 100000;
// std::vector<double> v(bench_count);
// viennamesh::LoggingStack s("bench");
//
// for (int i = 0; i != bench_count; ++i)
// v[i] = m_real.grad(i*0.1, i*0.2, 1e-2);
// }
info(1) << "After calculating sphere" << std::endl;
set_output("sphere", sphere);
set_output("mesh", mesh);
quantity_field_handle quantities = make_data<viennagrid::quantity_field>();
quantities.set(gradient_field_real);
set_output("sphere_quantities", quantities);
// m_real.print();
// std::cout << std::endl;
// std::cout << "m_real hast mirror symmetry: " << std::boolalpha << m_real.z_mirror_symmetry( mirror_symmetry_tolerance() ) << std::endl;
// m_real.rotation_symmetry_angles();
// // rotational_symmetry_tolerance() );
// std::cout << std::endl;
data_handle<viennamesh_point> rotation_vector = get_input<viennamesh_point>("rotation_vector");
data_handle<int> rotational_frequencies = get_input<int>("rotational_frequencies");
if (rotation_vector.valid())
{
for (int i = 0; i != rotation_vector.size(); ++i)
{
point new_z = rotation_vector(i);
info(1) << "Using rotation vector " << new_z << std::endl;
RealGeneralizedMoment rotated_m = m_real.get_rotated(new_z);
// rotated_m.print();
// std::cout << std::endl;
info(1) << "rotated_m (z = "<< new_z << ") hast mirror symmetry: " << std::boolalpha << rotated_m.z_mirror_symmetry( mirror_symmetry_tolerance() ) << std::endl;
// rotated_m.rotation_symmetry_angles();
// rotated_m.rotation_symmetry_angles( rotational_symmetry_tolerance() );
rotated_m.check_rotation_symmetry(M_PI);
if (rotational_frequencies.valid())
{
for (int i = 0; i != rotational_frequencies.size(); ++i)
{
int rotational_frequency = rotational_frequencies(i);
double angle = 2*M_PI/rotational_frequency;
info(1) << "Using rotational frequency " << rotational_frequency << " (angle = " << angle << ") error = " << rotated_m.check_rotation_symmetry(angle) << std::endl;
}
}
}
}
return true;
}
void generateVisualizationMarker(double steering_angle_rad, visualization_msgs::MarkerArray& marker_array)
{
double turn_radius = 0.0;
if (std::abs(steering_angle_rad) > 0.000001){
// use cotangent
//turn_radius = std::tan((M_PI*0.5 - steering_angle_rad) * p_wheel_base_);
turn_radius = (1.0/std::tan(steering_angle_rad)) * p_wheel_base_;
}
// (0,0) is rear axle middle
// x axis towards front, y axis to left of car
Eigen::Vector2d icc (0.0, turn_radius);
Eigen::Vector2d left_wheel (p_wheel_base_, p_wheel_track_*0.5);
Eigen::Vector2d right_wheel(p_wheel_base_, -p_wheel_track_*0.5);
double dist_left = (left_wheel - icc).norm();
double dist_right = (right_wheel - icc).norm();
double steer_angle_left = std::asin(p_wheel_base_/dist_left);
double steer_angle_right = std::asin(p_wheel_base_/dist_right);
if (turn_radius < 0){
steer_angle_left = -steer_angle_left;
steer_angle_right = -steer_angle_right;
}
ROS_DEBUG("turn radius: %f dist left: %f right: %f", turn_radius, dist_left, dist_right);
ROS_DEBUG("steer angle left: %f right: %f", steer_angle_left, steer_angle_right);
marker_array.markers[0].points.resize(40);
marker_array.markers[1].points.resize(40);
std::vector<geometry_msgs::Point>& point_vector_left = marker_array.markers[0].points;
std::vector<geometry_msgs::Point>& point_vector_right = marker_array.markers[1].points;
marker_array.markers[1].color.r = 0.0;
marker_array.markers[1].color.b = 1.0;
marker_array.markers[1].id = 1;
Eigen::Affine3d rot_left (Eigen::AngleAxisd(M_PI*0.5, Eigen::Vector3d::UnitX())*
Eigen::AngleAxisd(steer_angle_left, Eigen::Vector3d::UnitY()));
tf::quaternionEigenToMsg(Eigen::Quaterniond(rot_left.rotation()), marker_array_.markers[LEFT_FRONT_WHEEL].pose.orientation);
Eigen::Affine3d rot_right (Eigen::AngleAxisd(M_PI*0.5, Eigen::Vector3d::UnitX())*
Eigen::AngleAxisd(steer_angle_right, Eigen::Vector3d::UnitY()));
tf::quaternionEigenToMsg(Eigen::Quaterniond(rot_right.rotation()), marker_array_.markers[RIGHT_FRONT_WHEEL].pose.orientation);
//marker_array_.markers[LEFT_FRONT_WHEEL].pose.orientation; // = marker;
//marker_array_.markers[RIGHT_FRONT_WHEEL].pose.orientation; // = marker;
Eigen::Vector3d rotation_vector( Eigen::Vector3d::UnitZ() );
if (turn_radius > 0.0){
rotation_vector = -rotation_vector;
}
//std::cout << "rotation_vector:\n" << rotation_vector << "\n";
for (size_t i = 0; i < 40; ++i)
{
Eigen::Affine3d o_t_i (Eigen::Affine3d::Identity());
o_t_i.translation() = Eigen::Vector3d(icc.x(), -icc.y(), 0.0);
//Eigen::Rotation2Dd rotation(steer_angle_left + static_cast<double>(i) * 0.05);
Eigen::Affine3d rotation_left (Eigen::AngleAxisd(static_cast<double>(i) * 0.05,
rotation_vector));
//Eigen::Affine3d rotation_left (Eigen::AngleAxisd( static_cast<double>(i) * 0.05,
// (turn_radius > 0.0) ? -(Eigen::Vector3d::UnitZ()) : Eigen::Vector3d::UnitZ() ));
//Eigen::Vector2d tmp(o_t_i * rotation * left_wheel);
//Eigen::Vector2d tmp(o_t_i * rotation * left_wheel).translation();
Eigen::Vector3d tmp(o_t_i * rotation_left *Eigen::Vector3d(p_wheel_base_, (turn_radius)-p_wheel_track_*0.5, 0.0));
point_vector_left[i].x = tmp.x();
point_vector_left[i].y = -tmp.y();
Eigen::Affine3d rotation_right (Eigen::AngleAxisd(static_cast<double>(i) * 0.05,
rotation_vector));
//Eigen::Affine3d rotation_right (Eigen::AngleAxisd( static_cast<double>(i) * 0.05,
// (turn_radius > 0.0) ? -(Eigen::Vector3d::UnitZ()) : Eigen::Vector3d::UnitZ() ));
tmp = o_t_i * rotation_right*Eigen::Vector3d(p_wheel_base_, (turn_radius)+p_wheel_track_*0.5, 0.0);
point_vector_right[i].x = tmp.x();
point_vector_right[i].y = -tmp.y();
}
}
