test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
Map.cpp@5:19f24c363418, 2017-07-06 (annotated)
- Committer:
- Ludwigfr
- Date:
- Thu Jul 06 16:51:40 2017 +0000
- Revision:
- 5:19f24c363418
- Parent:
- 3:37345c109dfc
latest 6/7 17h51
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ludwigfr | 0:9f7ee7ed13e4 | 1 | #include "Map.hpp" |
Ludwigfr | 0:9f7ee7ed13e4 | 2 | |
Ludwigfr | 0:9f7ee7ed13e4 | 3 | Map::Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight){ |
Ludwigfr | 0:9f7ee7ed13e4 | 4 | this->widthRealMap=widthRealMap; |
Ludwigfr | 0:9f7ee7ed13e4 | 5 | this->heightRealMap=heightRealMap; |
Ludwigfr | 0:9f7ee7ed13e4 | 6 | this->nbCellWidth=nbCellWidth; |
Ludwigfr | 0:9f7ee7ed13e4 | 7 | this->nbCellHeight=nbCellHeight; |
Ludwigfr | 0:9f7ee7ed13e4 | 8 | this->sizeCellWidth=widthRealMap/(float)nbCellWidth; |
Ludwigfr | 0:9f7ee7ed13e4 | 9 | this->sizeCellHeight=heightRealMap/(float)nbCellHeight; |
Ludwigfr | 0:9f7ee7ed13e4 | 10 | |
Ludwigfr | 0:9f7ee7ed13e4 | 11 | this->cellsLogValues= new float*[nbCellWidth]; |
Ludwigfr | 0:9f7ee7ed13e4 | 12 | for(int i = 0; i < nbCellWidth; ++i) |
Ludwigfr | 0:9f7ee7ed13e4 | 13 | this->cellsLogValues[i] = new float[nbCellHeight]; |
Ludwigfr | 0:9f7ee7ed13e4 | 14 | |
Ludwigfr | 0:9f7ee7ed13e4 | 15 | this->initialLogValues= new float*[nbCellWidth]; |
Ludwigfr | 0:9f7ee7ed13e4 | 16 | for(int i = 0; i < nbCellWidth; ++i) |
Ludwigfr | 0:9f7ee7ed13e4 | 17 | this->initialLogValues[i] = new float[nbCellHeight]; |
Ludwigfr | 0:9f7ee7ed13e4 | 18 | |
Ludwigfr | 0:9f7ee7ed13e4 | 19 | this->fill_initialLogValues(); |
Ludwigfr | 3:37345c109dfc | 20 | this->fill_map_with_empty(); |
Ludwigfr | 0:9f7ee7ed13e4 | 21 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 22 | |
Ludwigfr | 0:9f7ee7ed13e4 | 23 | //fill initialLogValues with the values we already know (here the bordurs) |
Ludwigfr | 0:9f7ee7ed13e4 | 24 | void Map::fill_initialLogValues(){ |
Ludwigfr | 0:9f7ee7ed13e4 | 25 | //Fill map, we know the border are occupied |
Ludwigfr | 0:9f7ee7ed13e4 | 26 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 27 | for (int j = 0; j<this->nbCellHeight; j++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 28 | if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1) |
Ludwigfr | 0:9f7ee7ed13e4 | 29 | this->initialLogValues[i][j] = this->proba_to_log(1); |
Ludwigfr | 0:9f7ee7ed13e4 | 30 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 31 | this->initialLogValues[i][j] = this->proba_to_log(0.5); |
Ludwigfr | 0:9f7ee7ed13e4 | 32 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 33 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 34 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 35 | |
Ludwigfr | 3:37345c109dfc | 36 | |
Ludwigfr | 3:37345c109dfc | 37 | //fill initialLogValues with the values we already know (here the bordurs) |
Ludwigfr | 3:37345c109dfc | 38 | void Map::fill_initialLogValuesLab4(){ |
Ludwigfr | 3:37345c109dfc | 39 | //Fill map, we know the border are occupied |
Ludwigfr | 3:37345c109dfc | 40 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 3:37345c109dfc | 41 | for (int j = 0; j<this->nbCellHeight; j++) { |
Ludwigfr | 3:37345c109dfc | 42 | if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1) |
Ludwigfr | 3:37345c109dfc | 43 | this->initialLogValues[i][j] = this->proba_to_log(1); |
Ludwigfr | 3:37345c109dfc | 44 | else |
Ludwigfr | 3:37345c109dfc | 45 | this->initialLogValues[i][j] = this->proba_to_log(0.5); |
Ludwigfr | 3:37345c109dfc | 46 | } |
Ludwigfr | 3:37345c109dfc | 47 | } |
Ludwigfr | 3:37345c109dfc | 48 | } |
Ludwigfr | 3:37345c109dfc | 49 | |
Ludwigfr | 3:37345c109dfc | 50 | void Map::fill_map_with_empty(){ |
Ludwigfr | 0:9f7ee7ed13e4 | 51 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 52 | for (int j = 0; j<this->nbCellHeight; j++) { |
Ludwigfr | 3:37345c109dfc | 53 | this->cellsLogValues[i][j] = this->proba_to_log(0.5); |
Ludwigfr | 0:9f7ee7ed13e4 | 54 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 55 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 56 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 57 | |
Ludwigfr | 0:9f7ee7ed13e4 | 58 | //returns the probability [0,1] that the cell is occupied from the log valAue lt |
Ludwigfr | 0:9f7ee7ed13e4 | 59 | float Map::log_to_proba(float lt){ |
Ludwigfr | 0:9f7ee7ed13e4 | 60 | return 1-1/(1+exp(lt)); |
Ludwigfr | 0:9f7ee7ed13e4 | 61 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 62 | |
Ludwigfr | 0:9f7ee7ed13e4 | 63 | void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){ |
Ludwigfr | 0:9f7ee7ed13e4 | 64 | this->cellsLogValues[widthIndice][heightIndice]=this->cellsLogValues[widthIndice][heightIndice]+this->proba_to_log(proba)+this->initialLogValues[widthIndice][heightIndice];//map is filled as map[0][0] get the data for the point closest to the origin |
Ludwigfr | 0:9f7ee7ed13e4 | 65 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 66 | |
Ludwigfr | 0:9f7ee7ed13e4 | 67 | float Map::cell_width_coordinate_to_world(int i){ |
Ludwigfr | 0:9f7ee7ed13e4 | 68 | return this->sizeCellWidth/2+i*this->sizeCellWidth; |
Ludwigfr | 0:9f7ee7ed13e4 | 69 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 70 | |
Ludwigfr | 0:9f7ee7ed13e4 | 71 | float Map::cell_height_coordinate_to_world(int j){ |
Ludwigfr | 0:9f7ee7ed13e4 | 72 | return this->sizeCellHeight/2+j*this->sizeCellHeight; |
Ludwigfr | 0:9f7ee7ed13e4 | 73 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 74 | |
Ludwigfr | 0:9f7ee7ed13e4 | 75 | float Map::get_proba_cell(int widthIndice, int heightIndice){ |
Ludwigfr | 0:9f7ee7ed13e4 | 76 | return this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]); |
Ludwigfr | 0:9f7ee7ed13e4 | 77 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 78 | |
Ludwigfr | 0:9f7ee7ed13e4 | 79 | //returns the log value that the cell is occupied from the probability value [0,1] |
Ludwigfr | 0:9f7ee7ed13e4 | 80 | float Map::proba_to_log(float p){ |
Ludwigfr | 0:9f7ee7ed13e4 | 81 | return log(p/(1-p)); |
Ludwigfr | 0:9f7ee7ed13e4 | 82 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 83 | |
Ludwigfr | 5:19f24c363418 | 84 | void Map::fill_map_with_kalman_knowledge(){ |
Ludwigfr | 5:19f24c363418 | 85 | float xCenterRect=0; |
Ludwigfr | 5:19f24c363418 | 86 | float yCenterRect=0; |
Ludwigfr | 5:19f24c363418 | 87 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 5:19f24c363418 | 88 | for (int j = 0; j<this->nbCellHeight; j++) { |
Ludwigfr | 5:19f24c363418 | 89 | if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1) |
Ludwigfr | 5:19f24c363418 | 90 | this->cellsLogValues[i][j] = this->proba_to_log(1); |
Ludwigfr | 5:19f24c363418 | 91 | else{ |
Ludwigfr | 5:19f24c363418 | 92 | //rectangle in center 30 width, 20 height |
Ludwigfr | 5:19f24c363418 | 93 | if(((this->cell_width_coordinate_to_world(i) >= this->widthRealMap/2-15) && (this->cell_width_coordinate_to_world(i) <= this->widthRealMap/2+15)) && ((this->cell_height_coordinate_to_world(j) >= this->heightRealMap/2-10) && (this->cell_height_coordinate_to_world(j) <= this->heightRealMap/2+10)) ){ |
Ludwigfr | 5:19f24c363418 | 94 | this->cellsLogValues[i][j] = this->proba_to_log(1); |
Ludwigfr | 5:19f24c363418 | 95 | }else |
Ludwigfr | 5:19f24c363418 | 96 | this->cellsLogValues[i][j] = this->proba_to_log(0.5); |
Ludwigfr | 5:19f24c363418 | 97 | } |
Ludwigfr | 5:19f24c363418 | 98 | |
Ludwigfr | 5:19f24c363418 | 99 | } |
Ludwigfr | 5:19f24c363418 | 100 | } |
Ludwigfr | 5:19f24c363418 | 101 | } |
Ludwigfr | 5:19f24c363418 | 102 | |
Ludwigfr | 0:9f7ee7ed13e4 | 103 | /* |
Ludwigfr | 0:9f7ee7ed13e4 | 104 | |
Ludwigfr | 0:9f7ee7ed13e4 | 105 | float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){ |
Ludwigfr | 0:9f7ee7ed13e4 | 106 | return this->nbCellWidth*this->sizeCellWidth-robot_y; |
Ludwigfr | 0:9f7ee7ed13e4 | 107 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 108 | |
Ludwigfr | 0:9f7ee7ed13e4 | 109 | float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){ |
Ludwigfr | 0:9f7ee7ed13e4 | 110 | return robot_x; |
Ludwigfr | 0:9f7ee7ed13e4 | 111 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 112 | |
Ludwigfr | 0:9f7ee7ed13e4 | 113 | |
Ludwigfr | 0:9f7ee7ed13e4 | 114 | void MiniExplorerCoimbra::print_final_map() { |
Ludwigfr | 0:9f7ee7ed13e4 | 115 | float currProba; |
Ludwigfr | 0:9f7ee7ed13e4 | 116 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 117 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 0:9f7ee7ed13e4 | 118 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 119 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 0:9f7ee7ed13e4 | 120 | if ( currProba < 0.5) { |
Ludwigfr | 0:9f7ee7ed13e4 | 121 | pc.printf(" "); |
Ludwigfr | 0:9f7ee7ed13e4 | 122 | } else { |
Ludwigfr | 0:9f7ee7ed13e4 | 123 | if(currProba==0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 124 | pc.printf(" . "); |
Ludwigfr | 0:9f7ee7ed13e4 | 125 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 126 | pc.printf(" X "); |
Ludwigfr | 0:9f7ee7ed13e4 | 127 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 128 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 129 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 130 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 131 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 132 | |
Ludwigfr | 0:9f7ee7ed13e4 | 133 | |
Ludwigfr | 0:9f7ee7ed13e4 | 134 | void Map::print_final_map_with_robot_position(float robot_x,float robot_y) { |
Ludwigfr | 0:9f7ee7ed13e4 | 135 | float currProba; |
Ludwigfr | 0:9f7ee7ed13e4 | 136 | float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 137 | float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 138 | |
Ludwigfr | 0:9f7ee7ed13e4 | 139 | float heightIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 140 | float widthIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 141 | |
Ludwigfr | 0:9f7ee7ed13e4 | 142 | float widthMalus=-(3*sizeCellWidth/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 143 | float widthBonus=sizeCellWidth/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 144 | |
Ludwigfr | 0:9f7ee7ed13e4 | 145 | float heightMalus=-(3*sizeCellHeight/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 146 | float heightBonus=sizeCellHeight/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 147 | |
Ludwigfr | 0:9f7ee7ed13e4 | 148 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 149 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 0:9f7ee7ed13e4 | 150 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 151 | heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); |
Ludwigfr | 0:9f7ee7ed13e4 | 152 | widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); |
Ludwigfr | 0:9f7ee7ed13e4 | 153 | if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 0:9f7ee7ed13e4 | 154 | pc.printf(" R "); |
Ludwigfr | 0:9f7ee7ed13e4 | 155 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 156 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 0:9f7ee7ed13e4 | 157 | if ( currProba < 0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 158 | pc.printf(" "); |
Ludwigfr | 0:9f7ee7ed13e4 | 159 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 160 | if(currProba==0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 161 | pc.printf(" . "); |
Ludwigfr | 0:9f7ee7ed13e4 | 162 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 163 | pc.printf(" X "); |
Ludwigfr | 0:9f7ee7ed13e4 | 164 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 165 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 166 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 167 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 168 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 169 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 170 | |
Ludwigfr | 0:9f7ee7ed13e4 | 171 | void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) { |
Ludwigfr | 0:9f7ee7ed13e4 | 172 | float currProba; |
Ludwigfr | 0:9f7ee7ed13e4 | 173 | float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 174 | float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 175 | |
Ludwigfr | 0:9f7ee7ed13e4 | 176 | float heightIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 177 | float widthIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 178 | |
Ludwigfr | 0:9f7ee7ed13e4 | 179 | float widthMalus=-(3*sizeCellWidth/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 180 | float widthBonus=sizeCellWidth/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 181 | |
Ludwigfr | 0:9f7ee7ed13e4 | 182 | float heightMalus=-(3*sizeCellHeight/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 183 | float heightBonus=sizeCellHeight/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 184 | |
Ludwigfr | 0:9f7ee7ed13e4 | 185 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 186 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 0:9f7ee7ed13e4 | 187 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 188 | heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); |
Ludwigfr | 0:9f7ee7ed13e4 | 189 | widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); |
Ludwigfr | 0:9f7ee7ed13e4 | 190 | if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 0:9f7ee7ed13e4 | 191 | pc.printf(" R "); |
Ludwigfr | 0:9f7ee7ed13e4 | 192 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 193 | if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 0:9f7ee7ed13e4 | 194 | pc.printf(" T "); |
Ludwigfr | 0:9f7ee7ed13e4 | 195 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 196 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 0:9f7ee7ed13e4 | 197 | if ( currProba < 0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 198 | pc.printf(" "); |
Ludwigfr | 0:9f7ee7ed13e4 | 199 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 200 | if(currProba==0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 201 | pc.printf(" . "); |
Ludwigfr | 0:9f7ee7ed13e4 | 202 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 203 | pc.printf(" X "); |
Ludwigfr | 0:9f7ee7ed13e4 | 204 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 205 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 206 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 207 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 208 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 209 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 210 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 211 | */ |