00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #define DEBUG_
00021
00022 #include <ros/ros.h>
00023 #include <rosbag/bag.h>
00024 #include <rosbag/view.h>
00025
00026 #include <pcl/point_types.h>
00027 #include <pcl/point_cloud.h>
00028 #include <pcl/io/pcd_io.h>
00029 #include <pcl/registration/transforms.h>
00030
00031 #include <cob_3d_mapping_slam/curved_polygons/debug_interface.h>
00032
00033 #include <cob_3d_mapping_slam/slam/context.h>
00034 #include <cob_3d_mapping_slam/slam/node.h>
00035
00036 #include <cob_3d_mapping_slam/dof/tflink.h>
00037 #include <cob_3d_mapping_slam/dof/dof_uncertainty.h>
00038 #include <cob_3d_mapping_slam/dof/dof_variance.h>
00039
00040 #include <cob_3d_mapping_slam/slam/dummy/objctxt.h>
00041 #include <cob_3d_mapping_slam/slam/dummy/registration.h>
00042 #include <cob_3d_mapping_slam/slam/dummy/key.h>
00043 #include <cob_3d_mapping_slam/slam/dummy/robot.h>
00044
00045 #include <cob_3d_mapping_slam/curved_polygons/objctxt.h>
00046 #include <cob_3d_mapping_slam/curved_polygons/key.h>
00047
00048 #include <cob_3d_mapping_msgs/ShapeArray.h>
00049 #include "gnuplot_i.hpp"
00050 #include <gtest/gtest.h>
00051 #include <boost/random/mersenne_twister.hpp>
00052 #include <boost/random/uniform_real.hpp>
00053 #include <boost/random/variate_generator.hpp>
00054
00055 #define CYCLES 100
00056 static const char *BAGFILES[][512]={
00057
00058 {
00059
00060
00061
00062
00063
00064
00065 "test/cp3_rgbd_dataset_freiburg2_xyz.bag",
00066
00067
00068
00069
00070
00071
00072
00073 ""}
00074 };
00075
00076 static const char *GROUNDTRUTH[][512]={
00077 {
00078
00079
00080 "test/rgbd_dataset_freiburg2_xyz-groundtruth.txt",
00081
00082 0
00083 }
00084 };
00085
00086
00087 template<typename Matrix>
00088 float MATRIX_DISTANCE(const Matrix &a, const Matrix &b, const float thr=0.05) {
00089 Matrix c=a-b;
00090 float d=c.norm();
00091
00092 if(d>thr) {
00093 std::cout<<"A\n"<<a<<"\n";
00094 std::cout<<"B\n"<<b<<"\n";
00095 }
00096
00097 EXPECT_NEAR(d,0,thr);
00098 return d;
00099 }
00100
00101
00102
00103 void t2()
00104 {
00105 {
00106 typedef DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::DOF6_Uncertainty<Dummy::RobotParameters,float> > DOF6;
00107 typedef Slam::Node<Dummy::OBJCTXT<DOF6> > Node;
00108
00109 Slam::Context<Dummy::KEY, Node> ctxt(1,1);
00110 }
00111 {
00112 typedef DOF6::TFLinkvf DOF6;
00113 typedef Slam::Node<Dummy::OBJCTXT<DOF6> > Node;
00114
00115 Slam::Context<Dummy::KEY, Node> ctxt(1,1);
00116 }
00117
00118 }
00119
00120
00121 void t1()
00122 {
00123 typedef DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::DOF6_Uncertainty<Dummy::RobotParameters,float> > DOF6;
00124 typedef Slam::Node<Dummy::OBJCTXT<DOF6> > Node;
00125
00126 Slam::Context<Dummy::KEY, Node> ctxt(1,1);
00127
00128
00129 for(int i=0; i<CYCLES; i++) {
00130 ctxt.startFrame(i);
00131 for(int j=0; j<i; j++) {
00132 Node::OBJECT obj;
00133 ctxt+=obj.makeShared();
00134 }
00135 ctxt.finishFrame();
00136 }
00137
00138 }
00139
00140 float gen_random_float(float min, float max)
00141 {
00142 static boost::mt19937 rng;
00143 boost::uniform_real<float> u(min, max);
00144 boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > gen(rng, u);
00145 return gen();
00146 }
00147
00148
00149 std::vector<cob_3d_mapping_msgs::CurvedPolygon> generateRandomPlanes(const int N, const bool cors)
00150 {
00151 std::vector<cob_3d_mapping_msgs::CurvedPolygon> r;
00152
00153 for(int i=0; i<N; i++)
00154 {
00155 cob_3d_mapping_msgs::CurvedPolygon cp;
00156
00157 cp.ID = i;
00158
00159 cp.parameter[0] = gen_random_float(2.f, 4.f);
00160 cp.parameter[1] = gen_random_float(-1.f,1.f);
00161 cp.parameter[2] = 0;
00162 cp.parameter[3] = gen_random_float(-1.f,1.f);
00163 cp.parameter[4] = 0;
00164 cp.parameter[5] = 0;
00165
00166 Eigen::Vector3f z,n,np;
00167 z.fill(0);z(2)=1.f;
00168 n=z;
00169 n(0) = -cp.parameter[1];
00170 n(1) = -cp.parameter[3];
00171 n.normalize();
00172 np = cp.parameter[0]*(z.dot(n))*n;
00173
00174 cob_3d_mapping_msgs::feature ft;
00175 ft.ID = 1;
00176 ft.x = np(0);
00177 ft.y = np(1);
00178 ft.z = np(2);
00179 cp.features.push_back(ft);
00180
00181 if(cors)
00182 {
00183 cob_3d_mapping_msgs::simalarity_score ss;
00184 ss.ID = i;
00185 ss.prob = 1.f;
00186 cp.score.push_back(ss);
00187 }
00188
00189
00190 float size = gen_random_float(0.5f, 2.f)/10;
00191 for(int j=0; j<10; j++)
00192 {
00193 cob_3d_mapping_msgs::polyline_point pt;
00194 pt.edge_prob = 1;
00195
00196 pt.x = j*size;
00197 pt.y = 0;
00198 cp.polyline.push_back(pt);
00199
00200 pt.x = j*size;
00201 pt.y = size*10;
00202 cp.polyline.push_back(pt);
00203
00204 pt.y = j*size;
00205 pt.x = 0;
00206 cp.polyline.push_back(pt);
00207
00208 pt.y = j*size;
00209 pt.x = size*10;
00210 cp.polyline.push_back(pt);
00211 }
00212
00213 r.push_back(cp);
00214 }
00215
00216 return r;
00217 }
00218
00219 typedef void (*motion_fct)(Eigen::Matrix3f &, Eigen::Vector3f &);
00220
00221 void motion1(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00222 {
00223 rot = Eigen::Matrix3f::Identity();
00224 tr.fill(0);
00225 }
00226
00227 void motion2(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00228 {
00229 rot = Eigen::Matrix3f::Identity();
00230 tr(0) = gen_random_float(0,0.2f);
00231 tr(1) = gen_random_float(0,0.2f);
00232 tr(2) = gen_random_float(0,0.2f);
00233 }
00234
00235 void motion2s(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00236 {
00237 rot = Eigen::Matrix3f::Identity();
00238 tr = 0.1*Eigen::Vector3f::Identity();
00239 }
00240
00241 void motion3(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00242 {
00243 Eigen::Vector3f n;
00244 n(0) = gen_random_float(-1,1);
00245 n(1) = gen_random_float(-1,1);
00246 n(2) = gen_random_float(-1,1);
00247 n.normalize();
00248 Eigen::AngleAxisf aa(gen_random_float(-0.02f,0.05f),n);
00249 rot = aa.toRotationMatrix();
00250 tr.fill(0);
00251 }
00252
00253 void motion3s(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00254 {
00255 Eigen::Vector3f n=Eigen::Vector3f::Identity();
00256 Eigen::AngleAxisf aa(0.05f,n);
00257 rot = aa.toRotationMatrix();
00258 tr.fill(0);
00259 }
00260
00261 void motion4(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00262 {
00263 Eigen::Matrix3f r;
00264 Eigen::Vector3f t;
00265 motion2(r,tr);
00266 motion3(rot,t);
00267 }
00268
00269 void motion4s(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
00270 {
00271 Eigen::Matrix3f r;
00272 Eigen::Vector3f t;
00273 motion2s(r,tr);
00274 motion3s(rot,t);
00275 }
00276
00277
00278 void t4()
00279 {
00280
00281 motion_fct motion[]={motion1,motion2s,motion3s,motion4s,motion2,motion3,motion4,0};
00282
00283 int mp=0;
00284 while(motion[mp])
00285 {
00286 Eigen::Matrix3f path_rot=Eigen::Matrix3f::Identity(),rot=Eigen::Matrix3f::Identity(),tmp_rot,tmp_rot2;
00287 Eigen::Vector3f path_tr=Eigen::Vector3f::Zero(),tr=Eigen::Vector3f::Zero(),tmp_tr,tmp_tr2;
00288
00289
00290 std::vector<cob_3d_mapping_msgs::CurvedPolygon> planes = generateRandomPlanes(5, true);
00291
00292
00293 typedef DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::DOF6_Uncertainty<Dummy::RobotParameters,float> > DOF6;
00294 typedef Slam::Node<Slam_CurvedPolygon::OBJCTXT<DOF6> > Node;
00295
00296 Slam::Context<Slam_CurvedPolygon::KEY<DOF6>, Node> ctxt(1,1);
00297
00298
00299 for(int i=0; i<15; i++)
00300 {
00301 std::cout<<"real rot\n"<<rot<<"\n";
00302 std::cout<<"real tr\n"<<tr<<"\n";
00303 path_rot = rot*path_rot;
00304 path_tr += tr;
00305
00306 ctxt.startFrame(i);
00307
00308 for(size_t j=0; j<planes.size(); j++)
00309 {
00310 ctxt += planes[j];
00311 }
00312
00313 ctxt.finishFrame();
00314
00315
00316 motion[mp](rot,tr);
00317
00318 for(size_t j=0; j<planes.size(); j++)
00319 {
00320 Eigen::Vector3f v,t;
00321 v(0)=planes[j].features[0].x;
00322 v(1)=planes[j].features[0].y;
00323 v(2)=planes[j].features[0].z;
00324 v=rot*v;
00325 t=v*(tr.dot(v))/(v.squaredNorm());
00326 ROS_ASSERT(t.squaredNorm()<=tr.squaredNorm());
00327 v+=t;
00328 planes[j].features[0].x=v(0);
00329 planes[j].features[0].y=v(1);
00330 planes[j].features[0].z=v(2);
00331
00332 Eigen::Vector3f n=v;
00333
00334 planes[j].parameter[0] = n.norm();
00335 n/=n(2);
00336
00337 planes[j].parameter[1] = -n(0);
00338 planes[j].parameter[3] = -n(1);
00339 Eigen::Vector3f z;
00340 z.fill(0);z(2)=1.f;
00341 n.normalize();
00342 planes[j].parameter[0] /= (n.dot(z));
00343 }
00344
00345
00346 tmp_rot = Eigen::Matrix3f::Identity();
00347 tmp_tr = Eigen::Vector3f::Zero();
00348 tmp_rot2 = Eigen::Matrix3f::Identity();
00349 tmp_tr2 = Eigen::Vector3f::Zero();
00350 const Slam::SWAY<Node> *n = &ctxt.getPath().getLocal();
00351 while(n)
00352 {
00353 tmp_tr += n->link_.getTranslation();
00354 tmp_rot = ((Eigen::Matrix3f)n->link_.getRotation())*tmp_rot;
00355
00356 tmp_tr2 += tmp_rot2.inverse()*n->link_.getSource1()->getTranslation();
00357 tmp_rot2 = ((Eigen::Matrix3f)n->link_.getSource1()->getRotation())*tmp_rot2;
00358
00359 std::cout<<"con\n";
00360 n = n->node_->getConnections().size()?&n->node_->getConnections()[0]:NULL;
00361 }
00362 std::cout<<"ROT1\n"<<tmp_rot<<"\n";
00363 std::cout<<"ROT2\n"<<tmp_rot2<<"\n";
00364 std::cout<<"ROT3\n"<<path_rot<<"\n";
00365 std::cout<<"TR1\n"<<tmp_tr<<"\n";
00366 std::cout<<"TR2\n"<<tmp_tr2<<"\n";
00367 std::cout<<"TR3\n"<<path_tr<<"\n";
00368 MATRIX_DISTANCE(tmp_rot, path_rot);
00369 MATRIX_DISTANCE(tmp_tr, path_tr);
00370
00371 }
00372
00373 mp++;
00374 }
00375
00376 }
00377
00378
00379 TEST(Slam,merging)
00380 {
00381
00382 cob_3d_mapping_msgs::feature ft;
00383 cob_3d_mapping_msgs::polyline_point pt;
00384 cob_3d_mapping_msgs::CurvedPolygon s;
00385
00386 {
00387 s.weight = 100;
00388 s.parameter[0] = 2;
00389 s.parameter[1] = 0;
00390 s.parameter[2] = -0.01;
00391 s.parameter[3] = 0;
00392 s.parameter[4] = -0.01;
00393 s.parameter[5] = 0;
00394 s.ID = 0;
00395 s.energy = "";
00396 s.features.clear();
00397 pt.edge_prob = 1;
00398
00399 pt.x = -1;
00400 pt.y = -1;
00401 s.polyline.push_back(pt);
00402 pt.x = 1;
00403 pt.y = -1;
00404 s.polyline.push_back(pt);
00405 pt.x = 1;
00406 pt.y = 1;
00407 s.polyline.push_back(pt);
00408 pt.x = -1;
00409 pt.y = 1;
00410 s.polyline.push_back(pt);
00411
00412 ft.ID = 1;
00413 ft.x = 0;
00414 ft.y = 0;
00415 ft.z = 2;
00416 s.features.push_back(ft);
00417
00418 ft.ID = 2;
00419 ft.x = 0;
00420 ft.y = 0;
00421 ft.z = 0;
00422 s.features.push_back(ft);
00423
00424 ft.ID = 3;
00425 ft.x = 0;
00426 ft.y = 0;
00427 ft.z = 2;
00428 s.features.push_back(ft);
00429
00430 ft.ID = 4;
00431 ft.x = 1;
00432 ft.y = 0;
00433 ft.z = 0;
00434 s.features.push_back(ft);
00435
00436 ft.ID = 5;
00437 ft.x = 1;
00438 ft.y = 0;
00439 ft.z = 0;
00440 s.features.push_back(ft);
00441 }
00442 Slam_CurvedPolygon::ex_curved_polygon xp1 = s;
00443 EXPECT_FALSE( xp1.invalid() );
00444
00445 {
00446 s.parameter[0] = 2;
00447 s.parameter[1] = 0;
00448 s.parameter[2] = 0;
00449 s.parameter[3] = 0;
00450 s.parameter[4] = 0;
00451 s.parameter[5] = 0;
00452
00453 s.ID = 1;
00454 s.features.clear();
00455
00456 for(size_t i=0; i<s.polyline.size(); i++)
00457 {
00458 s.polyline[i].x += 0.5;
00459 }
00460
00461 ft.ID = 1;
00462 ft.x = 0.5;
00463 ft.y = 0;
00464 ft.z = 2;
00465 s.features.push_back(ft);
00466
00467 ft.ID = 2;
00468 ft.x = 0;
00469 ft.y = 0;
00470 ft.z = 0;
00471 s.features.push_back(ft);
00472
00473 ft.ID = 3;
00474 ft.x = 0.5;
00475 ft.y = 0;
00476 ft.z = 2;
00477 s.features.push_back(ft);
00478
00479 ft.ID = 4;
00480 ft.x = 1;
00481 ft.y = 0;
00482 ft.z = 0;
00483 s.features.push_back(ft);
00484
00485 ft.ID = 5;
00486 ft.x = 1;
00487 ft.y = 0;
00488 ft.z = 0;
00489 s.features.push_back(ft);
00490 }
00491
00492 Slam_CurvedPolygon::ex_curved_polygon xp2 = s;
00493 EXPECT_FALSE( xp2.invalid() );
00494
00495 Eigen::Vector3f t;
00496 t(0) = 0;
00497 t(1) = 0;
00498 t(2) = 0;
00499 Eigen::Matrix3f R = Eigen::Matrix3f::Identity();
00500
00501
00502 xp2.transform(R, t, 0.1,0.1);
00503
00504 std::cout<<xp1.getFeatures()[5].v_<<"\n\n";
00505 std::cout<<xp1.getFeatures()[6].v_<<"\n\n";
00506 std::cout<<xp1.getFeatures()[7].v_<<"\n\n";
00507 std::cout<<xp1.getFeatures()[8].v_<<"\n\n";
00508 std::cout<<"----------\n";
00509 std::cout<<xp2.getFeatures()[5].v_<<"\n\n";
00510 std::cout<<xp2.getFeatures()[6].v_<<"\n\n";
00511 std::cout<<xp2.getFeatures()[7].v_<<"\n\n";
00512 std::cout<<xp2.getFeatures()[8].v_<<"\n\n";
00513 std::cout<<"----------\n";
00514
00515 std::cout<<"can "<<(!xp1.canMerge(xp2)?"not ":" ")<<"merge\n";
00516
00517 xp1 += xp2;
00518
00519 std::cout<<xp1.getFeatures()[5].v_<<"\n\n";
00520 std::cout<<xp1.getFeatures()[6].v_<<"\n\n";
00521 std::cout<<xp1.getFeatures()[7].v_<<"\n\n";
00522 std::cout<<xp1.getFeatures()[8].v_<<"\n\n";
00523
00524 Eigen::Vector3f r;
00525 r(2) = 2;
00526
00527 r(0) = -1.5;
00528 r(1) = 1;
00529 EXPECT_NEAR( (xp1.getFeatures()[5].v_-r).norm(), 0, 0.05 );
00530 r(0) = 1;
00531 r(1) = 1;
00532 EXPECT_NEAR( (xp1.getFeatures()[6].v_-r).norm(), 0, 0.05 );
00533 r(0) = 1;
00534 r(1) = -1;
00535 EXPECT_NEAR( (xp1.getFeatures()[7].v_-r).norm(), 0, 0.05 );
00536 r(0) = -1.5;
00537 r(1) = -1;
00538 EXPECT_NEAR( (xp1.getFeatures()[8].v_-r).norm(), 0, 0.05 );
00539 }
00540
00541
00542 struct SOdomotry_Data
00543 {
00544 double timestamp;
00545 Eigen::Vector3f t;
00546 Eigen::Quaternionf q;
00547 };
00548
00549 #include <visualization_msgs/Marker.h>
00550 #include <nav_msgs/Odometry.h>
00551 #include "pcl/visualization/pcl_visualizer.h"
00552 #include <pcl/visualization/cloud_viewer.h>
00553 TEST(Slam,bag_run)
00554
00555 {
00556 pcl::visualization::CloudViewer viewer("abc");
00557 pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
00558
00559 int bg_file=0;
00560 while(strlen(BAGFILES[0][bg_file])>0) {
00561
00562 std::vector<SOdomotry_Data> odos;
00563 if(GROUNDTRUTH[0][bg_file]) {
00564 std::ifstream infile(GROUNDTRUTH[0][bg_file]);
00565
00566 std::string line;
00567 while (std::getline(infile, line))
00568 {
00569 if(line.size()<1 || line[0]=='#') continue;
00570
00571 std::istringstream iss(line);
00572 SOdomotry_Data odo;
00573 if (!(iss >> odo.timestamp >> odo.t(0) >> odo.t(1) >> odo.t(2) >> odo.q.x() >> odo.q.y() >> odo.q.z() >> odo.q.w())) {
00574 ROS_ERROR("parsing groundtruth");
00575 ROS_ASSERT(0);
00576 break;
00577 }
00578
00579 odos.push_back(odo);
00580 }
00581
00582 }
00583
00584 rosbag::Bag bag, bag_out;
00585 try {
00586
00587 bag. open(BAGFILES[0][bg_file], rosbag::bagmode::Read);
00588 bag_out.open(std::string(BAGFILES[0][bg_file])+".odo.bag", rosbag::bagmode::Write);
00589
00590
00591 std::vector<std::string> topics;
00592 topics.push_back(std::string("curved_polygons"));
00593 topics.push_back(std::string("/curved_polygons"));
00594 topics.push_back(std::string("shapes_array"));
00595 topics.push_back(std::string("/shapes_array"));
00596
00597 rosbag::View view(bag, rosbag::TopicQuery(topics));
00598
00599 visualization_msgs::Marker marker_text, marker_points, marker_planes, marker_cor1, marker_cor2, marker_del, marker_map;
00600 marker_text.header.frame_id = "/openni_rgb_frame";
00601 marker_text.pose.position.x = 0;
00602 marker_text.pose.position.y = 0;
00603 marker_text.pose.position.z = 0;
00604 marker_text.pose.orientation.x = 0.0;
00605 marker_text.pose.orientation.y = 0.0;
00606 marker_text.pose.orientation.z = 0.0;
00607 marker_text.pose.orientation.w = 1.0;
00608 marker_text.action = visualization_msgs::Marker::ADD;
00609 marker_text.type = visualization_msgs::Marker::LINE_LIST;
00610 marker_text.scale.x = marker_text.scale.y = marker_text.scale.z = 0.01;
00611 marker_text.color.r = marker_text.color.g = marker_text.color.b = marker_text.color.a = 1;
00612
00613 marker_map = marker_del = marker_cor1 = marker_cor2 = marker_points = marker_planes = marker_text;
00614 marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00615 marker_text.scale.x = marker_text.scale.y = marker_text.scale.z = 0.35;
00616 marker_points.color.g = marker_points.color.b = 0;
00617 marker_planes.color.g = marker_planes.color.r = 0;
00618 marker_cor2.color.b = marker_cor2.color.r = 0;
00619 marker_del.action = visualization_msgs::Marker::DELETE;
00620 marker_map.type = visualization_msgs::Marker::TRIANGLE_LIST;
00621 marker_map.scale.x = marker_map.scale.y = marker_map.scale.z = 1;
00622
00623 marker_text.id = 0;
00624 marker_points.id = 1;
00625 marker_planes.id = 2;
00626 marker_cor1.id = 3;
00627 marker_cor2.id = 4;
00628 marker_map.id = 5;
00629
00630
00631 typedef DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::DOF6_Uncertainty<Dummy::RobotParameters,float> > DOF6;
00632 typedef Slam::Node<Slam_CurvedPolygon::OBJCTXT<DOF6> > Node;
00633
00634 Slam::Context<Slam_CurvedPolygon::KEY<DOF6>, Node> ctxt(.60,.30);
00635
00636 Eigen::Vector3f last_tr = Eigen::Vector3f::Zero();
00637 Eigen::Matrix3f last_rot= Eigen::Matrix3f::Identity();
00638
00639 Eigen::Vector3f last_odo_tr = Eigen::Vector3f::Zero();
00640 Eigen::Vector3f first_odo_tr = Eigen::Vector3f::Zero();
00641 Eigen::Matrix3f first_odo_rot= Eigen::Matrix3f::Identity();
00642
00643
00644 int ctr=0;
00645 ros::Time last, start;
00646 bool first=true;
00647 std::vector<cob_3d_mapping_msgs::ShapeArray> memory_sa;
00648 BOOST_FOREACH(rosbag::MessageInstance const m, view)
00649 {
00650
00651 cob_3d_mapping_msgs::ShapeArray::ConstPtr sa = m.instantiate<cob_3d_mapping_msgs::ShapeArray>();
00652 if (sa != NULL)
00653 {
00654 memory_sa.push_back(*sa);
00655 continue;
00656 }
00657
00658 geometry_msgs::Point line_p, line_p0;
00659 line_p0.x = line_p0.y = line_p0.z = 0;
00660
00661 cob_3d_mapping_msgs::CurvedPolygon::ConstPtr s = m.instantiate<cob_3d_mapping_msgs::CurvedPolygon>();
00662 if (s != NULL)
00663 {
00664
00665
00666
00667 if(last!=s->stamp) {
00668 marker_cor1.header.stamp = marker_cor2.header.stamp = marker_points.header.stamp = marker_planes.header.stamp = marker_text.header.stamp = s->stamp;
00669
00670 ROS_INFO("new frame");
00671
00672 last=s->stamp;
00673 if(first) {
00674 first=false;
00675 start = s->stamp;
00676 }
00677 else
00678 ctxt.finishFrame();
00679
00680 ROS_INFO("timestamp %f (%d)", (last-start).toSec(), ctr);
00681 ctr++;
00682
00683 pcl::PointXYZRGB p;
00684 Eigen::Vector3f from,to,temp;
00685 while(Debug::Interface::get().getArrow(from,to,p.r,p.g,p.b))
00686 {
00687 line_p.x = from(0);
00688 line_p.y = from(1);
00689 line_p.z = from(2);
00690 p.r>100?marker_cor1.points.push_back(line_p):marker_cor2.points.push_back(line_p);
00691 line_p.x = to(0);
00692 line_p.y = to(1);
00693 line_p.z = to(2);
00694 p.r>100?marker_cor1.points.push_back(line_p):marker_cor2.points.push_back(line_p);
00695 temp = to-from;
00696
00697
00698 for(float ms=0; ms<=1; ms+=0.01)
00699 {
00700 p.x=ms*temp(0)+from(0);
00701 p.y=ms*temp(1)+from(1);
00702 p.z=ms*temp(2)+from(2);
00703 if(p.getVector3fMap().squaredNorm()<100)
00704 rgb->push_back(p);
00705 }
00706 }
00707
00708
00709
00710
00711 Eigen::Matrix3f tmp_rot = Eigen::Matrix3f::Identity();
00712 Eigen::Matrix3f tmp_rot2 = Eigen::Matrix3f::Identity();
00713 Eigen::Vector3f tmp_tr = Eigen::Vector3f::Zero();
00714 const Slam::SWAY<Node> *n = &ctxt.getPath().getLocal();
00715 while(n)
00716 {
00717
00718 tmp_tr = tmp_rot2*tmp_tr + n->link_.getTranslation();
00719 tmp_rot = ((Eigen::Matrix3f)n->link_.getRotation())*tmp_rot;
00720 tmp_rot2= ((Eigen::Matrix3f)n->link_.getRotation());
00721
00722 std::cout<<"con\n";
00723 n = n->node_->getConnections().size()?&n->node_->getConnections()[0]:NULL;
00724 }
00725 std::cout<<"ROT1\n"<<(::DOF6::EulerAnglesf)tmp_rot<<"\n";
00726 std::cout<<"TR1\n"<<tmp_tr<<"\n";
00727
00728 Eigen::Quaternionf quat((Eigen::Matrix3f)tmp_rot.inverse());
00729
00730 nav_msgs::Odometry odo;
00731 odo.header.frame_id = "/openni_rgb_frame";
00732 odo.header.stamp = last;
00733 odo.pose.pose.position.x = -tmp_tr(0);
00734 odo.pose.pose.position.y = -tmp_tr(1);
00735 odo.pose.pose.position.z = -tmp_tr(2);
00736 odo.pose.pose.orientation.x = quat.x();
00737 odo.pose.pose.orientation.y = quat.y();
00738 odo.pose.pose.orientation.z = quat.z();
00739 odo.pose.pose.orientation.w = quat.w();
00740
00741 bag_out.write("odometry", last, odo);
00742
00743 while(odos.size()>0 && odos.front().timestamp<last.toSec())
00744 odos.erase(odos.begin());
00745 if(odos.size()>0)
00746 {
00747 if(start == s->stamp)
00748 {
00749 first_odo_tr = odos.front().t;
00750 first_odo_rot = odos.front().q;
00751 }
00752
00753 odos.front().t -= first_odo_tr;
00754 odos.front().t = first_odo_rot.inverse()*odos.front().t;
00755 odos.front().q = first_odo_rot.inverse()*odos.front().q;
00756
00757 std::cout<<"ROT2\n"<<(::DOF6::EulerAnglesf)tmp_rot<<"\n";
00758 std::cout<<"TR2\n"<<tmp_tr<<"\n";
00759
00760 std::cout<<"ODO ROT2\n"<<(::DOF6::EulerAnglesf)odos.front().q.toRotationMatrix().inverse()<<"\n";
00761 std::cout<<"ODO TR2\n"<<-odos.front().t<<"\n";
00762
00763 odo.header.stamp = ros::Time(odos.front().timestamp);
00764 odo.pose.pose.position.x = odos.front().t(0);
00765 odo.pose.pose.position.y = odos.front().t(1);
00766 odo.pose.pose.position.z = odos.front().t(2);
00767
00768 ROS_INFO("odometry tr jump with length %f", (odos.front().t-last_odo_tr).norm());
00769 last_odo_tr = odos.front().t;
00770 odo.pose.pose.orientation.x = odos.front().q.x();
00771 odo.pose.pose.orientation.y = odos.front().q.y();
00772 odo.pose.pose.orientation.z = odos.front().q.z();
00773 odo.pose.pose.orientation.w = odos.front().q.w();
00774
00775 bag_out.write("groundtruth", ros::Time(odos.front().timestamp), odo);
00776 }
00777
00778 tmp_rot = Eigen::Matrix3f::Identity();
00779 tmp_rot2 = Eigen::Matrix3f::Identity();
00780 tmp_tr = Eigen::Vector3f::Zero();
00781 n = &ctxt.getPath().getLocal();
00782 while(n)
00783 {
00784
00785 tmp_tr = tmp_rot2*tmp_tr + n->link_.getTranslation();
00786 tmp_rot = ((Eigen::Matrix3f)n->link_.getRotation())*tmp_rot;
00787 tmp_rot2= ((Eigen::Matrix3f)n->link_.getRotation());
00788
00789 for(size_t i=0; i<n->node_->getContext().getObjs().size(); i++)
00790 {
00791 if( n->node_->getContext().getObjs()[i]->getUsedCounter()<7)
00792 continue;
00793
00794 std::vector<Eigen::Vector3f> tris;
00795 n->node_->getContext().getObjs()[i]->getData().getTriangles(tris);
00796 ROS_ASSERT(tris.size()%3==0);
00797
00798 ::std_msgs::ColorRGBA col;
00799 unsigned int rnd=(((i+1)*11)<<12)+(i+3)*i*7;
00800 col.a=1;
00801 col.r = ((rnd>>0)&0xff)/255.f;
00802 col.g = ((rnd>>8)&0xff)/255.f;
00803 col.b = ((rnd>>16)%0xff)/255.f;
00804 for(size_t j=0; j<tris.size(); j++)
00805 {
00806
00807
00808 Eigen::Vector3f v=tmp_rot*tris[j]+tmp_tr;
00809 line_p.x = v(0);
00810 line_p.y = v(1);
00811 line_p.z = v(2);
00812 marker_map.points.push_back(line_p);
00813 marker_map.colors.push_back(col);
00814 }
00815 }
00816
00817 n = n->node_->getConnections().size()?&n->node_->getConnections()[0]:NULL;
00818
00819 }
00820
00821 while(memory_sa.size()>0 && memory_sa.front().header.stamp<=last)
00822 {
00823 bag_out.write("shapes_array", last, memory_sa.front());
00824 memory_sa.erase(memory_sa.begin());
00825 }
00826 for(int i=0; i<6; i++)
00827 {
00828 marker_del.id = i;
00829 if(i<5) bag_out.write("/markers", last, marker_del);
00830 else bag_out.write("/map", last, marker_del);
00831 }
00832 bag_out.write("/markers", last, marker_points);
00833 bag_out.write("/markers", last, marker_planes);
00834 bag_out.write("/markers", last, marker_cor1);
00835 bag_out.write("/markers", last, marker_cor2);
00836 bag_out.write("/map", last, marker_map);
00837 marker_points.points.clear();
00838 marker_planes.points.clear();
00839 marker_cor1.points.clear();
00840 marker_cor2.points.clear();
00841 marker_map.points.clear();
00842 marker_map.colors.clear();
00843
00844
00845
00846 if((last_tr-tmp_tr).norm()>0.2 || ((::DOF6::EulerAnglesf)(last_rot.inverse()*tmp_rot)).getVector().squaredNorm()>0.15f*0.15f) {
00847 ROS_INFO("BIG JUMP");
00848 ROS_ERROR("BIG JUMP");
00849
00850 marker_text.text = "BIG JUMP";
00851 bag_out.write("/markers", last, marker_text);
00852
00853
00854 }
00855 last_tr=tmp_tr;
00856 last_rot=tmp_rot;
00857
00858 rgb->width=1;
00859 rgb->height=rgb->size();
00860 if(rgb->size())
00861 {
00862
00863
00864 viewer.showCloud(rgb);
00865 std::cerr<<"press any key\n";
00866
00867 if(getchar()=='q')
00868 {
00869 bag_out.close();
00870 return;
00871 }
00872 rgb.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
00873 }
00874
00875 ctxt.startFrame(s->stamp.toSec());
00876 }
00877 }
00878
00879 if(s->weight<50) continue;
00880
00881 Slam_CurvedPolygon::ex_curved_polygon xp = *s;
00882
00883 if(xp.invalid())
00884 continue;
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900 ctxt += *s;
00901
00902 line_p.x = xp.getNearestPoint()(0);
00903 line_p.y = xp.getNearestPoint()(1);
00904 line_p.z = xp.getNearestPoint()(2);
00905 if(xp.isPlane())
00906 {
00907 marker_planes.points.push_back(line_p0);
00908 marker_planes.points.push_back(line_p);
00909 }
00910 else
00911 {
00912 marker_points.points.push_back(line_p0);
00913 marker_points.points.push_back(line_p);
00914 }
00915
00916 pcl::PointXYZRGB p;
00917 std::cerr<<"param: "
00918 <<s->parameter[0]<<" "
00919 <<s->parameter[1]<<" "
00920 <<s->parameter[2]<<" "
00921 <<s->parameter[3]<<" "
00922 <<s->parameter[4]<<" "
00923 <<s->parameter[5]<<"\n";
00924 for(float ms=0; ms<=1; ms+=0.01)
00925 {
00926 p.r=p.g=p.b=0;
00927 if(!xp.isPlane())
00928 p.r=255;
00929 else
00930 p.b=255;
00931 p.x=ms*xp.getNearestPoint()(0);
00932 p.y=ms*xp.getNearestPoint()(1);
00933 p.z=ms*xp.getNearestPoint()(2);
00934 if(p.getVector3fMap().squaredNorm()<100)
00935 rgb->push_back(p);
00936 }
00937
00938 Eigen::Vector3f temp;
00939 temp=xp.getFeatures()[1].v_;
00940 temp.normalize();
00941 temp*=0.1;
00942 for(float ms=0; ms<=1; ms+=0.01)
00943 {
00944 p.r=p.g=p.b=0;
00945 p.g=255;
00946 p.b=255;
00947 p.x=ms*temp(0)+xp.getNearestPoint()(0);
00948 p.y=ms*temp(1)+xp.getNearestPoint()(1);
00949 p.z=ms*temp(2)+xp.getNearestPoint()(2);
00950 if(p.getVector3fMap().squaredNorm()<100)
00951 rgb->push_back(p);
00952 }
00953 temp=xp.getFeatures()[3].v_;
00954 temp.normalize();
00955 temp*=0.1;
00956 for(float ms=0; ms<=1; ms+=0.01)
00957 {
00958 p.r=p.g=p.b=0;
00959 p.r=255;
00960 p.b=255;
00961 p.x=ms*temp(0)+xp.getNearestPoint()(0);
00962 p.y=ms*temp(1)+xp.getNearestPoint()(1);
00963 p.z=ms*temp(2)+xp.getNearestPoint()(2);
00964 if(p.getVector3fMap().squaredNorm()<100)
00965 rgb->push_back(p);
00966 }
00967 temp=xp.getFeatures()[4].v_;
00968 temp.normalize();
00969 temp*=0.1;
00970 for(float ms=0; ms<=1; ms+=0.01)
00971 {
00972 p.r=p.g=p.b=0;
00973 p.r=255;
00974 p.g=255;
00975 p.x=ms*temp(0)+xp.getNearestPoint()(0);
00976 p.y=ms*temp(1)+xp.getNearestPoint()(1);
00977 p.z=ms*temp(2)+xp.getNearestPoint()(2);
00978 if(p.getVector3fMap().squaredNorm()<100)
00979 rgb->push_back(p);
00980 }
00981
00982 }
00983
00984 ctxt.finishFrame();
00985
00986 bag.close();
00987 }
00988 catch(...) {
00989 bag_out.close();
00990 std::cout<<"failed to load: "<<BAGFILES[0][bg_file]<<"\n";
00991 ASSERT_TRUE(false);
00992 }
00993
00994 bg_file++;
00995 }
00996 }
00997
00998 int main(int argc, char **argv){
00999 ros::Time::init();
01000
01001 testing::InitGoogleTest(&argc, argv);
01002 return RUN_ALL_TESTS();
01003 }