00001 #include <stdio.h>
00002 #include <string>
00003 #ifdef _WIN32
00004 #include <Windows.h>
00005 #else
00006 #include <unistd.h>
00007 #endif
00008
00009
00010 #include <octomap/octomap.h>
00011 #include <octomap/OcTreeStamped.h>
00012 #include <octomap/math/Utils.h>
00013 #include "testing.h"
00014
00015 using namespace std;
00016 using namespace octomap;
00017 using namespace octomath;
00018
00019 int main(int argc, char** argv) {
00020
00021 if (argc != 2){
00022 std::cerr << "Error: you need to specify a test as argument" << std::endl;
00023 return 1;
00024 }
00025 std::string test_name (argv[1]);
00026
00027
00028
00029 if (test_name == "MathVector") {
00030
00031 Vector3* twos = new Vector3();
00032 Vector3* ones = new Vector3(1,1,1);
00033 for (int i=0;i<3;i++) {
00034 (*twos)(i) = 2;
00035 }
00036
00037 Vector3 subtraction = *twos - *ones;
00038 Vector3 addition = *twos + *ones;
00039 Vector3 multiplication = *twos * 2.;
00040
00041 for (int i=0;i<3;i++) {
00042 EXPECT_FLOAT_EQ (subtraction(i), 1.);
00043 EXPECT_FLOAT_EQ (addition(i), 3.);
00044 EXPECT_FLOAT_EQ (multiplication(i), 4.);
00045 }
00046
00047
00048 Vector3 rotation = *ones;
00049
00050
00051 rotation.rotate_IP (M_PI, 1., 0.1);
00052 EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
00053 EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
00054 EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
00055
00056
00057 } else if (test_name == "MathPose") {
00058
00059 Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
00060 Pose6D b;
00061
00062 Vector3 trans(1.0f, 0.1f, 0.1f);
00063 Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
00064 Pose6D c(trans, rot);
00065
00066
00067 EXPECT_TRUE ( a == c);
00068
00069 EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
00070
00071
00072 Vector3 t = c.transform (trans);
00073 EXPECT_FLOAT_EQ (t.x() , 1.6399229);
00074 EXPECT_FLOAT_EQ (t.y() , 0.8813442);
00075 EXPECT_FLOAT_EQ (t.z() , 0.099667005);
00076
00077
00078 Pose6D c_inv = c.inv();
00079 Vector3 t2 = c_inv.transform (t);
00080 EXPECT_FLOAT_EQ (t2.x() , trans.x());
00081 EXPECT_FLOAT_EQ (t2.y() , trans.y());
00082 EXPECT_FLOAT_EQ (t2.z() , trans.z());
00083
00084
00085 } else if (test_name == "InsertRay") {
00086 double p = 0.5;
00087 EXPECT_FLOAT_EQ(p, probability(logodds(p)));
00088 p = 0.1;
00089 EXPECT_FLOAT_EQ(p, probability(logodds(p)));
00090 p = 0.99;
00091 EXPECT_FLOAT_EQ(p, probability(logodds(p)));
00092
00093 float l = 0;
00094 EXPECT_FLOAT_EQ(l, logodds(probability(l)));
00095 l = -4;
00096 EXPECT_FLOAT_EQ(l, logodds(probability(l)));
00097 l = 2;
00098 EXPECT_FLOAT_EQ(l, logodds(probability(l)));
00099
00100
00101 OcTree tree (0.05);
00102 tree.setProbHit(0.7);
00103 tree.setProbMiss(0.4);
00104
00105 point3d origin (0.01f, 0.01f, 0.02f);
00106 point3d point_on_surface (2.01f,0.01f,0.01f);
00107
00108 for (int i=0; i<360; i++) {
00109 for (int j=0; j<360; j++) {
00110 EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
00111 point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
00112 }
00113 point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
00114 }
00115 EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
00116 EXPECT_EQ ((int) tree.size(), 50615);
00117
00118
00119
00120
00121
00122
00123
00124
00125 } else if (test_name == "InsertScan") {
00126 Pointcloud* measurement = new Pointcloud();
00127
00128 point3d origin (0.01f, 0.01f, 0.02f);
00129 point3d point_on_surface (2.01f, 0.01f, 0.01f);
00130
00131 for (int i=0; i<360; i++) {
00132 for (int j=0; j<360; j++) {
00133 point3d p = origin+point_on_surface;
00134 measurement->push_back(p);
00135 point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
00136 }
00137 point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
00138 }
00139
00140 OcTree tree (0.05);
00141 tree.insertPointCloud(*measurement, origin);
00142 EXPECT_EQ (tree.size(), 53959);
00143
00144 ScanGraph* graph = new ScanGraph();
00145 Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f);
00146 graph->addNode(measurement, node_pose);
00147 EXPECT_TRUE (graph->writeBinary("test.graph"));
00148 delete graph;
00149
00150
00151 } else if (test_name == "ReadGraph") {
00152 ScanGraph graph;
00153 EXPECT_TRUE (graph.readBinary("test.graph"));
00154
00155
00156 } else if (test_name == "StampedTree") {
00157 OcTreeStamped stamped_tree (0.05);
00158
00159 for (int x=-20; x<20; x++)
00160 for (int y=-20; y<20; y++)
00161 for (int z=-20; z<20; z++) {
00162 point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
00163 stamped_tree.updateNode(p, true);
00164 }
00165
00166 point3d query (0.1f, 0.1f, 0.1f);
00167 OcTreeNodeStamped* result = stamped_tree.search (query);
00168 EXPECT_TRUE (result);
00169 unsigned int tree_time = stamped_tree.getLastUpdateTime();
00170 unsigned int node_time = result->getTimestamp();
00171 std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
00172 EXPECT_TRUE (tree_time > 0);
00173 #ifdef _WIN32
00174 Sleep(1000);
00175 #else
00176 sleep(1);
00177 #endif
00178 stamped_tree.integrateMissNoTime(result);
00179 std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
00180 EXPECT_EQ (node_time, result->getTimestamp());
00181 point3d query2 = point3d (0.1f, 0.1f, 0.3f);
00182 stamped_tree.updateNode(query2, true);
00183 OcTreeNodeStamped* result2 = stamped_tree.search (query2);
00184 EXPECT_TRUE (result2);
00185 result = stamped_tree.search (query);
00186 EXPECT_TRUE (result);
00187 std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp()
00188 << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
00189 EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp());
00190 EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
00191
00192 } else if (test_name == "OcTreeKey") {
00193 OcTree tree (0.05);
00194 point3d p(0.0,0.0,0.0);
00195 OcTreeKey key;
00196 tree.coordToKeyChecked(p, key);
00197 point3d p_inv = tree.keyToCoord(key);
00198 EXPECT_FLOAT_EQ (0.025, p_inv.x());
00199 EXPECT_FLOAT_EQ (0.025, p_inv.y());
00200 EXPECT_FLOAT_EQ (0.025, p_inv.z());
00201
00202
00203 } else {
00204 std::cerr << "Invalid test name specified: " << test_name << std::endl;
00205 return 1;
00206
00207 }
00208
00209 std::cerr << "Test successful.\n";
00210 return 0;
00211 }