Udacity AI for Robotics translating Python code to C++

Question

I've been learning C++ for about a week now. My primary language is Python, so I thought I would find some code to translate to figure out how the pieces fit together. Please let me know any simplifications/best practices!

There is an assignment on a unit of Udacity's "Artificial Intelligence for Robotics" where you have a probability distribution for a 2D array of states, and given sensory input and movement, you calculate the probability of being in any of those states. The original Python code is first, and my C++ revision is below:

Edit: I'll attempt to give more detail on what the problem is, as well as add some comments to the original code.

There is a 4x5 map (not the CS sense of the word) of possible states, which you can think of as a checkerboard or similar with colors that correspond to the colors array. Our sensor is somewhere on the map, but we don't know where.

Before we sense anything about our environment we have a belief that any position is equally likely, which initializes our probability distribution p (probDist in the C++ code) to 0.05, or 1 in 20 squares. We will then update our belief by shifting, then sensing, in that order, 5 times.

By shift, I mean that the sensor attempts to move according to the #Motion comment at the top of the Python code. The sensor has a p_move (probMove in the C++ code) probability of successfully moving, otherwise it remains in the same location. What the shift function does is it convolves the current probability distribution with the probability distribution of successful movement. In other words, it figures out where it believes it will be after an attempted input motion.

The sense function is simply measuring the color of the square the sensor is on, aware of the fact that the sensor could be incorrect. The sensor_right (probSensorCorrect in the C++ code) amount is the probability of the sensor giving the correct color of the current square. The Law of Total Probability is then used to update the current belief by first multiplying each square by the probability that the square would be sensed as the measurement color and then dividing each probability by the sum of all to ensure they add to 1 (normalizing the probabilities).

The answer in the Python comments is what was given by Udacity, and was reproduced by both my Python and C++ code below. Hope that clears things up a bit... :D

edit 2: Things that feel wrong, but I'm not sure how to address

I'm passing the arrays by reference, but I have to copy anyway because each field of the modified probDist array depends on multiple fields of the original probDist array. I am declaring constants in an attempt to constrain the problem, but not sure if I'm using the correct syntax, as well as if I should remove them from main() entirely... I'm also trying to use the STL to get used to Modern C++, but is there any reason I would use a C array? Thanks.

# Motion:
# [0,0] - stay
# [0,1] - right
# [0,-1] - left
# [1,0] - down
# [-1,0] - up
def localize(colors,measurements,motions,sensor_right,p_move):
 # initializes p to a uniform distribution over a grid of the same dimensions as colors
 pinit = 1.0 / float(len(colors)) / float(len(colors[0]))
 p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]
 
 #shifts and senses n times
 for move in range(len(motions)):
 p = shift(p, motions[move], p_move)
 p = sense(p, colors, measurements[move], sensor_right)
 
 return p
 
def sense(p, colors, measurement, sensor_right):
 """updates belief based on color measurement"""
 rowNum = len(colors)
 colNum = len(colors[0])
 q = [[] for _ in range(rowNum)]
 sum = 0
 
 for row in range(rowNum):
 for col in range(colNum):
 if measurement == colors[row][col]:
 prob = sensor_right
 else:
 prob = 1 - sensor_right
 q_piece = (p[row][col])*prob
 q[row].append(q_piece)
 sum += q_piece
 
 for row in range(rowNum):
 for col in range(colNum):
 q[row][col] = q[row][col]/sum
 
 return q
 
 
def shift(p, motion, p_move):
 """updates belief based on movement attempt"""
 rowNum = len(p)
 colNum = len(p[0])
 q = [[] for _ in range(rowNum)]
 
 for row in range(rowNum):
 for col in range(colNum):
 q[row].append((1-p_move)*(p[row][col]))
 q[row][col] += p_move*p[(row-motion[0])%rowNum][(col-motion[1])%colNum]
 
 return q
 
def show(p):
 rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]
 print '[' + ',\n '.join(rows) + ']'
 
#############################################################
# For the following test case, your output should be 
# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],
# [0.00715, 0.01017, 0.08696, 0.07988, 0.00935],
# [0.00739, 0.00894, 0.11272, 0.35350, 0.04065],
# [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]
# (within a tolerance of +/- 0.001 for each entry)
colors = [['R','G','G','R','R'],
 ['R','R','G','R','R'],
 ['R','R','G','G','R'],
 ['R','R','R','R','R']]
measurements = ['G','G','G','G','G']
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)
show(p) # displays your answer

C++:

#include <iostream>
#include <array>
using namespace std;
void sense(
 array<array<double, 5>, 4> &probDist,
 const array<array<char, 5>, 4> &colors,
 const char measurement,
 const double probSensorCorrect)
{
 //Update PD based on sensory input
 array<array<double, 5>, 4> newProbDist;
 int rows = probDist.size();
 int cols = probDist[0].size();
 double probPiece{0};
 double sum{0};
 //Update each position with its
 //non-normalized probability
 //and build the normalizer
 for (int row=0; row!=rows; ++row) {
 for (int col=0; col!=cols; ++col) {
 if (measurement == colors[row][col])
 probPiece = probSensorCorrect;
 else
 probPiece = 1.0 - probSensorCorrect;
 probPiece *= probDist[row][col];
 newProbDist[row][col] = probPiece;
 sum += probPiece;
 }
 }
 //Normalize each
 for (int row=0; row!=rows; ++row) {
 for (int col=0; col!=cols; ++col) {
 newProbDist[row][col] /= sum;
 }
 }
 probDist = newProbDist;
}
void shift(
 array<array<double, 5>, 4> &probDist,
 const array<int, 2> &motion,
 const double probMove)
{
 //Update PD based on move attempt
 array<array<double, 5>, 4> newProbDist;
 int rows = probDist.size();
 int cols = probDist[0].size();
 int rowCalc;
 int colCalc;
 //Update each position with its probability
 for (int row=0; row!=rows; ++row) {
 for (int col=0; col!=cols; ++col) {
 //Probability of not moving and staying
 newProbDist[row][col] =
 (1.0 - probMove)*probDist[row][col];
 //Plus probability of moving from another
 rowCalc = (((row - motion[0])%rows) + rows)%rows;
 colCalc = (((col - motion[1])%cols) + cols)%cols;
 newProbDist[row][col] +=
 probMove*probDist[rowCalc][colCalc];
 }
 }
 probDist = newProbDist;
}
array<array<double, 5>, 4> localize(
 const array<array<char, 5>, 4> &colors,
 const array<char, 5> &measurements,
 const array<array<int, 2>, 5> &motions,
 const double probSensorCorrect,
 const double probMove)
{
 //Localize the sensor among the 4x5 grid
 //Initialize Probability Distribution
 //as a Uniform Distribution
 array<array<double, 5>, 4> probDist;
 float rows = colors.size();
 float cols = colors[0].size();
 double probInit = 1.0/rows/cols; 
 for (uint row=0; row!=rows; ++row) {
 for (uint col=0; col!=cols; ++col) {
 probDist[row][col] = probInit;
 }
 }
 //Shift and sense n times
 for (int move=0; move!=measurements.size(); ++move) {
 shift(probDist, motions[move], probMove);
 sense(probDist, colors, measurements[move], probSensorCorrect);
 }
 return probDist;
}
int main()
{
 //Initialize Constants
 array<array<double, 5>, 4> probDist;
 const array<array<char, 5>, 4> colors{{
 {'R', 'G', 'G', 'R', 'R'},
 {'R', 'R', 'G', 'R', 'R'},
 {'R', 'R', 'G', 'G', 'R'},
 {'R', 'R', 'R', 'R', 'R'},
 }};
 const array<char , 5> measurements{
 'G', 'G', 'G', 'G', 'G',
 };
 const array<array<int, 2>, 5> motions{{
 {0, 0}, {0, 1}, {1, 0}, {1, 0}, {0, 1},
 }};
 const double probSensorCorrect = 0.7;
 const double probMove = 0.8;
 //Localize the sensor and print PD
 probDist = localize(colors, measurements,
 motions, probSensorCorrect, probMove);
 for (auto row: probDist) {
 for (double element: row) {
 cout << ' ' << element << ' ';
 }
 cout << endl;
 }
 return 0;
}

• 200_success: Could you give us a bit of an explanation for why that the case should produce that output matrix?
• miscco: As a general remark, try to wait some time before you accept an answer. There might be some guy who has some additional insight.

Answer 1

This is quite an interesting piece of code.

There are however some things i would change.

1. Do not use namespace std; It is bad practice that will cost you in the long run, so avoid it early on.

2. While it is a good representation of your physical world, I cannot see a real benefit for utilizing a nested 2D array. In most cases your code does not rely on the position within the grid and when it does you do boundchecks anyway. So i would suggest, that you simplify it to

   std::array<double, 5 * 4> matrix;
   

3. Stick to your codestyle. Generally, when you already use braces for nesting, use them everywhere.

4. Initializing non array variables via double sum{0}; is really unorthodox. Stick with the simple double sum = 0.0;

5. Separate operators like == with spaces that makes the code much more readable.

6. Use the available functionality. Stuff like int rows = probDist.size(); is unneeded. Simply use probDist.size() in your code.

7. For simple if-else assignments the ternary operator ? might be more suitable, e.g.

   double probPiece = (measurement == colors[row][col]) ?
    probSensorCorrect : 1.0 - probSensorCorrect;
   

8. Declare variables where you use them, e.g. rowCal and colCalc in the shift function are only used inside the for loop-

9. Your sense function does not need to preserve the old array so directly overwrite it rather than assigning a temporary.

   void sense(
    std::array<double, 5 * 4> &probDist,
    const std::array<char, 5 * 4> &colors,
    const char measurement,
    const double probSensorCorrect)
   {
    double sum = 0.0;
    for (unsigned i = 0; i < probDist.size(); ++i) {
    probDist[i] *= (measurement == colors[i]) ?
    probSensorCorrect : 1.0 - probSensorCorrect;
    sum += probDist[i];
    }
    for (auto &probability : probDist) {
    probability /= sum;
    }
   }
   

• Camsbury: Thanks so much! If I don't coerce probDist.size() to an int as I have above, will things still go according to plan when using a modulus as in (row - motion[0])%rows, seeing as .size() is of a different type - or does it not matter and is always coerced to the left hand operand?
• Camsbury: It seems also that readability is increased if I use int rows = probDist.size();. A lot of the code would blow up if I didn't have those in.
• Camsbury: Also - can you give me an example of point 3? Struggling to find a standard style for C++ coming from Python, where it is so obvious...
• miscco: I would say, that readability is not really increased. Consider you have multiple arrays with different sizes, then you will get in trouble. This way you directly know what it is. Also note that you will always have to double check whether rows actually is what it promises. This is not the case for array.size(). Regarding point 3, I meant the missing braces around the if statement in the sense function
• Camsbury: Great thanks. I'll post a different block soon and try to implement these changes.