19 int main(
int argc,
char** argv) {
22 std::cerr <<
"Error: you need to specify a test as argument" << std::endl;
25 std::string test_name (argv[1]);
29 if (test_name ==
"MathVector") {
33 for (
int i=0;i<3;i++) {
37 Vector3 subtraction = *twos - *ones;
38 Vector3 addition = *twos + *ones;
39 Vector3 multiplication = *twos * 2.;
41 for (
int i=0;i<3;i++) {
57 }
else if (test_name ==
"MathPose") {
59 Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (
float)
M_PI/4. );
62 Vector3 trans(1.0f, 0.1f, 0.1f);
85 }
else if (test_name ==
"InsertRay") {
105 point3d origin (0.01f, 0.01f, 0.02f);
106 point3d point_on_surface (2.01f,0.01f,0.01f);
108 for (
int i=0; i<360; i++) {
109 for (
int j=0; j<360; j++) {
125 }
else if (test_name ==
"InsertScan") {
128 point3d origin (0.01f, 0.01f, 0.02f);
129 point3d point_on_surface (2.01f, 0.01f, 0.01f);
131 for (
int i=0; i<360; i++) {
132 for (
int j=0; j<360; j++) {
133 point3d p = origin+point_on_surface;
135 point_on_surface.rotate_IP (0,0,
DEG2RAD(1.));
145 Pose6D node_pose (origin.
x(), origin.
y(), origin.
z(),0.0f,0.0f,0.0f);
146 graph->addNode(measurement, node_pose);
151 }
else if (test_name ==
"ReadGraph") {
156 }
else if (test_name ==
"StampedTree") {
159 for (
int x=-20; x<20; x++)
160 for (
int y=-20; y<20; y++)
161 for (
int z=-20; z<20; z++) {
162 point3d p ((
float) x*0.05f+0.01f, (
float) y*0.05f+0.01f, (
float) z*0.05f+0.01f);
166 point3d query (0.1f, 0.1f, 0.1f);
171 std::cout <<
"After 1st update (cube): Tree time " <<tree_time <<
"; node(0.1, 0.1, 0.1) time " << result->
getTimestamp() << std::endl;
179 std::cout <<
"After 2nd update (single miss): Tree time " <<tree_time <<
"; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
185 result = stamped_tree.
search (query);
187 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()
188 <<
"; node(0.1, 0.1, 0.3) time " << result2->
getTimestamp() << std::endl;
192 }
else if (test_name ==
"OcTreeKey") {
204 std::cerr <<
"Invalid test name specified: " << test_name << std::endl;
209 std::cerr <<
"Test successful.\n";
std::istream & readBinary(std::ifstream &s)
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void integrateMissNoTime(OcTreeNodeStamped *node) const
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
double probability(double logodds)
compute probability from logodds:
NODE * search(double x, double y, double z, unsigned int depth=0) const
int main(int argc, char **argv)
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
void push_back(float x, float y, float z)
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
double keyToCoord(unsigned short int key, unsigned depth) const
Vector3 & rotate_IP(double roll, double pitch, double yaw)
unsigned int getLastUpdateTime()
Pose6D inv() const
Inversion.
unsigned int getTimestamp() const
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
#define EXPECT_FLOAT_EQ(a, b)
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
bool writeBinary(const std::string &filename)
This class represents a tree-dimensional pose of an object.
This class represents a three-dimensional vector.
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
float logodds(double probability)
compute log-odds from probability:
#define EXPECT_TRUE(args)
virtual size_t size() const
This class represents a Quaternion.